perm filename ARM.PAL[PNT,HE]5 blob
sn#611879 filedate 1981-09-09 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00103 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00012 00002 PDP11/45 ARM SERVO PROGRAM: REVISIONS
C00017 00003 Conditional assembly flags
C00022 00004 Definitions
C00026 00005 PUMA definitions:
C00031 00006 MACROS for establishing relative pointers for DEVICE DATA BLOCK
C00036 00007 MACROS to establish pointer tables into arm and hardware device SERVO DATA BLOCKS
C00040 00008 Pointer tables for servo data
C00044 00009 Servo function error codes & bit masks for servo, status ans mode words
C00049 00010 Organization of SERVO DATA BLOCK
C00058 00011 Data list structures for polynomial coefficients and DEVICE BLOCKS
C00063 00012 SERVO - level 6 servo code
C00077 00013 SVLOOP - subroutine of "servo"
C00089 00014 STPMVE, STPRUN, CATERR - Stopping joint motion routines
C00095 00015 ANGLES - converts A/D readings to joint angle and velocity
C00102 00016 FEEDBK - servo feedback loop to drive the joint motors
C00109 00017 EVAL - fetches polynomial coef. and evaluates for next servicing time
C00126 00018 WIPER - switches A/D channels if A/D out of range & servo has multiple wipers
C00130 00019 MOTDRV, MOTSTP, BRKREL - motor drive routines
C00135 00020 INTARM - initializes servo code for future arm motions
C00144 00021 FINARM - called by AL to kill servo level 6 job at end of user program
C00145 00022 DRIVE - initiates motions for one joint, single segment
C00152 00023 CENTER - closes the hand fingers & centers the arm over any grasped object
C00163 00024 CTRLV6 - center touch sensor monitor
C00169 00025 Data for center operations
C00172 00026 WHERE - updates the current JT angle and dynamic coef for servos
C00176 00027 MOVE - moves the arm joints along a precomputed trajectory
C00184 00028 MOVE - diagnostic move instruction without trajectory modification
C00191 00029 ATTSRV - routine for attaching servos
C00204 00030 RELSRV - routine for detaching servos
C00207 00031 SETSET - resets servo block variables after a move is completed
C00209 00032 SETTH - reads A/D and initializes joint angles for a given device block
C00218 00033 SETREF - read A/D, set reference voltage and zero tach value readings
C00226 00034 STHLV6 & SRFLV6 - level 6 code for "SETTH" AND "SETREF"
C00233 00035 RUNSRV - schedules the servos of a device to run and waits for their completion
C00245 00036 DACSER - blue arm DAC refresh level 4 routine and arm error trap routine
C00247 00037 PUMA - Arm driver routine for PUMA manipulator
C00250 00038 EVPUMA - Evaluates the new position that the PUMA must servo to
C00257 00039 MOVPUM: Moves the PUMA.
C00271 00040 PANGLE - reads current PUMA joint angles
C00273 00041 TOANG, TOCODE - converts to/from PUMA encoder to degrees
C00277 00042 CHKANG - checks to make sure within joint range
C00280 00043 WVECT - writes a vector to the PUMA microprocessors
C00283 00044 RVECT - reads a vector from the PUMA microprocessor
C00286 00045 WSINGL, RSINGL - read and write single word to/from PUMA micro's
C00288 00046 BITON & BITOFF routines
C00290 00047 CALIB - Puma calibration routine
C00297 00048 PUMINI: Initializes the arm
C00299 00049 GOCAL: The main part of the calibration routine
C00304 00050 SETENC routine
C00308 00051 RPOT routine
C00310 00052 ERRPRN: Puma error print routine
C00315 00053 TOUCH - schedules touch events
C00329 00054 CLRTCH - takes a scheduled event off of the touch sensor event lists
C00332 00055 SMPTCH, STPTCH - initiates & terminates updating of the touch sensor readings
C00334 00056 TCHLV6 - level 6 code for touch sensors
C00341 00057 FORCE - force sensing and compliance subroutines
C00344 00058 SETC - initializes force sensing/compliance system
C00348 00059 FRCSIG - initializes job starting based upon a force reading
C00353 00060 FRCOFF - takes a queued job off of the force signal list
C00357 00061 BISON - sets up bias force of a given magnitude and direction
C00360 00062 BISOFF - turns off force compliance in a specified direction
C00364 00063 FRCLV6 - level 6 code for force sensing and compliance
C00379 00064 FBACK - level 5 force sensing background job
C00397 00065 WRIST SENSOR definitions
C00398 00066 SETBAS - sets the base readings for the force wrist
C00405 00067 WRIST - reads and resolves the force wrist readings
C00412 00068 GATHER - collects force data
C00420 00069 FORCE SENSING ROUTINES DATA AREA
C00430 00070 FFORCE - reads finger force values
C00434 00071 SETSTF - initializes stiffness system
C00439 00072 KSRVO - services all joints once per tick
C00448 00073 SS2 - computes trajectory deviation and determines force to apply
C00451 00074 SS1 - computes force compensation
C00453 00075 REPS - reads raw force sensor data
C00456 00076 TCI, TSI - computes commanded and sensed torque in I-th joint
C00459 00077 GJTCAL, FINKTH - computes torque transform and stiffness matrices
C00466 00078 DATA for stiffness servo system
C00470 00079 OPERATE - setsup screwdriver or vise operation
C00476 00080 OPRLV6 - operates screwdriver or vise
C00482 00081 VARIABLES, CONSTANTS, AND TEMPORARY STORAGE AREA COMMON TO ALL SERVOS
C00484 00082 SRV01: YELLOW ARM JOINT #1 DATA
C00487 00083 SRV02: YELLOW ARM JOINT #2 DATA
C00490 00084 SRV03: YELLOW ARM JOINT #3 DATA
C00493 00085 SRV04: YELLOW ARM JOINT #4 DATA
C00496 00086 SRV05: YELLOW ARM JOINT #5 DATA
C00499 00087 SRV06: YELLOW ARM JOINT #6 DATA
C00503 00088 SRV07: YELLOW ARM HAND SERVO DATA BLOCK
C00506 00089 SRV08: 0 BLUE ARM JOINT #1 DATA
C00509 00090 SRV09: 0 BLUE ARM JOINT #2 DATA
C00512 00091 SRV10: 0 BLUE ARM JOINT #3 DATA
C00515 00092 SRV11: 0 BLUE ARM JOINT #4 DATA
C00518 00093 SRV12: 0 BLUE ARM JOINT #5 DATA
C00521 00094 SRV13: 0 BLUE ARM JOINT #6 DATA
C00524 00095 SRV14: 0 BLUE ARM HAND SERVO DATA BLOCK
C00527 00096 SRV15: 0 VISE SERVO DATA BLOCK
C00530 00097 SRV16: 0 SCREWDRIVER SERVO DATA BLOCK
C00533 00098 SRV17: 0 Green PUMA arm data block
C00540 00099 SRV18: green hand servo data block
C00541 00100 SRV19: 0 Red PUMA arm data block
C00547 00101 SRV20: Red hand servo data block
C00549 00102 PUMA servo constants for microprocessor-based joint servos:
C00553 00103 DIAGNOSTIC DATA BUFFER: ORGANIZATION AND LOCAL STORAGE
C00561 ENDMK
C⊗;
; PDP11/45 ARM SERVO PROGRAM: REVISIONS
.TITLE ARM
.EVEN
;September 9, 1981:
; After both PUMAs were retrofitted for 6 axes, both now use the complete
; 6-axis arm solution.
;March 2, 1981:
; Both Red and Green PUMAs are now controllable from AL.
;February 7,1981:
; All level 7 code moved to level 6, level 7 no longer used.
;December 17, 1980:
; PUMA driver routines added to control Green PUMA arm under AL.
;October 20, 1980:
; SERVO now runs all the devices in 1 process which is now 16 ms.
;May 1, 1980:
; SERVO, KSERVO changed to run all servos in device block as 1 process.
; Force and touch sensing level 7 jobs are now treated as subroutines.
; Kernel now ticks at 8 ms.
;April 27, 1980:
; MOVE altered to eliminate runtime trajectory modification
;Jan 3, 1980:
; GATHER altered to pass back ID # in integer buf.
;Feb 9, 1979:
; GATHER and force gathering system made operational
;Jan. 19, 1979:
; WRIST and SETBASE made operational
;Aug. 31, 1978:
; DTERMS in BEJCZY.PAL modified to explicitly zero gravity loading for
;joint 1 and 6.
;May 11, 1978:
; Added call to where at beginning of center to permit manual positioning
;of arm prior to center. Fixed velocity error nulling bug in SERVO.
;December 6, 1977:
; Panic Button interupt service routine now returns via a new kernel
;system call, KRTI, permitting interupt returns from outside supervisor mode.
;November 6, 1977
; Added filtering to force signalling
;March 14, 1977:
; Changed Blue arm interface dac refresh routine to level 4 from 7
;and removed restriction on running only one device at a time.
;March 8, 1977:
; Re-calibrated blue arm after all dead tachs repaired and arm
;generally re-built.
;October 27, 1976:
; Changed some symbols to locals, put conditional compilation
;switches on linear calibration data and drive. Added WOBBLE.
;August 2,1976:
; Power supply drifting by as much as 4 counts, upped reference
;error tolerance to ignor for now.
;May 12, 1976:
; Adding procedures WRIST and SETBAS for reading and resolving
;the force wrist strain gage values.
;April 23, 1976:
; Bug in FEEDBACK subroutine corrected. Used to store the error
;torque ( ERRTQE ) in funny units, not oz-in and oz's. Also changed
;EVAL so that the inertia torque left in AC3 was in the proper units.
;March 25,1976:
; KV,KE,KI changed for BLUE arm, joint #2. ERRTOL changed from
;.05 to .15 for joint #2, .01 to .02 for joint #3, due mainly to a hardware
;bug in the ADC. Only odd ADC readings seem to be stable for these joints.
;March 20, 1976:
; Arm code uses new Kernel, does its own ADC reading. Order of
;execution altered so that polynomial evaluation is done before the
;pots and tachs are read and before the error feedback is computed.
;Conditional assembly flags
;MACRO FOR DEFINING UNDEFINED CONDITIONAL ASSEMBLY SWITCHES
.MACRO DSWT SWTCH,DEFULT
.IFNDF SWTCH
.PRINT /"SWTCH" ASSEMBLY SWITCH NOT DEFINED, USING DEFAULT VALUE
/
SWTCH==DEFULT
.ENDC
.ENDM
DSWT DIAGY,1 ;DIAGNOSTIC MODE, JOINT VARIABLES SAVED IN BUFFER DBUF
DSWT STDALN,1 ;STAND ALONE, DOES NOT REQUIRE AL GRAPH ROUTINES
DSWT TACCAL,0 ;MODE TO CALIBRATE THE TACHOMETERS
DSWT NOYELW,1 ;NO YELLOW ARM YET, DON'T CREATE SERVO DATA BLOCKS
DSWT TIMER,0 ;SAVES EXECUTION TIME OF SERVO CODE IN ARRAY "TIMES"
DSWT ISLIN,1 ;JOINTS ASSUMED TO BE LINEAR, DON'T DO NON-LINEAR INTERP.
DSWT SHORT,0 ;SET TO 1 ELIMINATE UNNECESSARY STUFF FOR SENSOR DIAGNOSTICS
DSWT FFING,0 ;SET TO 1 TO USE FORCE SENSING FINGERS
DSWT NOPHND,1 ;SET TO 0 IF PUMAS HAVE REAL HANDS.
;HEADER SECTION FOR ASSEMBLING "ARM" FOR OVERLAYING WITH AL ROUTINES
.IFZ STDALN
.OFFSET -160000 ;Put it in the high memory
.IF1
.INSRT K1DEF.PAL[11,SYS]
.INSRT ALHEAD.PAL[AL,HE]
.ENDC
.OFFSET -340000 ;Put it in the high memory
.=ARMCOD ;START ADDRESS OF SERVO CODE
CODE$ == ARMCOD
DATA$ == ARMDAT
SPSWITCH == 0 ;Make sure we start off with everyone properly defined
DATA
.BLKW 96. ;RESERVE SPACE FOR WRIST CALIBRATION DATA
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARMSOL.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
DATA
; Following are pointers to routines and data defined in this program.
; These go into the COMTAB.
PUTLOC ARMVER,VERSION ;TELL AL WHAT VERSION NUMBER THE BIN FILE IS
; Procedures in this module:
PUTLOC LINTARM,INTARM ;ESTABLISH POINTERS TO ROUTINES THAT AL CALLS
PUTLOC LFINARM,FINARM
PUTLOC LCENTER,CENTER
PUTLOC LWHERE,WHERE
PUTLOC LMOVE,MOVE
PUTLOC LCALIB,CALIB
PUTLOC LSETC,SETC
PUTLOC LFRCSIG,FRCSIG
PUTLOC LFRCOFF,FRCOFF
PUTLOC LBISON,BISON
PUTLOC LBISOFF,BISOFF
PUTLOC LSETBAS,SETBAS
PUTLOC LWRIST,WRIST
PUTLOC LGATHER,GATHER
PUTLOC LSETSTF,SETSTF
PUTLOC LOPERATE,OPERATE
; Pointers to data blocks in this module (defined on page 8 of this file):
PUTLOC LERRPTR,ERRPTR
PUTLOC LTHPTR,THPTR
PUTLOC LDVCPTR,DVCPTR
PUTLOC LPOTPTR,POTPTR
; From BEJCZY.PAL:
PUTLOC LDTERMS,DTERMS
; Arithmetic routines from ARITH.PAL
PUTLOC LSQRTF,SQRTF
PUTLOC LEXP,EXP
PUTLOC LLOG,LOG
PUTLOC LSNCSD,SNCOS
PUTLOC LASIN,ASIN
PUTLOC LACOS,ACOS
PUTLOC LATAN2,ATAN2
; Arm solution routines in ARMSOL.PAL:
PUTLOC LUPDATE,UPDATE
PUTLOC LSOLVE,SOLVE
.ENDC
;DATA POINTER DEFS. *K
IIII == .
.==NOTB11
.WORD 0
PCDBUF::.WORD 0 ;pcode buffer address
INTBUF::.WORD 0 ;integer buffer starting address
FPBUF:: .WORD 0 ;floating point buffer starting address
INTPTR::.WORD 0 ;current position of integer pointer
FPPTR:: .WORD 0 ;current position of fp buffer
INTSIZ::.WORD 0 ;size of integer buffer
FPSIZ:: .WORD 0 ;size of fp buffer
.==IIII
;END OF HEADER SECTION FOR OVERLAYING WITH AL ROUTINES
;Definitions
AC ==%0 ;GENERAL ACCUMULATOR
BC ==%1 ; " "
CC ==%2 ; " "
DC ==%3 ; " "
EC ==%4 ; " "
DAT ==%5 ;CONTAINS ADDRESS OF FIRST WORD OF SERVO DATA
FC ==%5
SP =%6 ;STACK POINTER
PC =%7 ;PROGRAM COUNTER
;NUMBER REPRESENTATION OF VARIABLES AND CONSTANTS:
;
;JOINT ANGLES: DEGREES
;ANGULAR VELOCITY: DEGREES/MILLISECOND
;ANGULAR ACCELERATION: DEGREES/MILLISECOND**2
;TIME: TICKS {APPROXIMATELY 1 MILLISECOND}
;TORQUE: OZ-INCHES OR OZ
;PROCESSOR PRIORITY LEVELS CURRENTLY BEING USED:
; LEVEL 7: NOT USED
; LEVEL 6: SERVO JOBS, FORCE SENSING TRIGGER JOB, TOUCH SENSING
; TRIGGER JOB, SETTH ADC READER, SETREF ADC READER
; LEVEL 5: CENTER ON-MONITOR, FORCE SENSING AND COMPLIANCE CALCULATOR
; LEVEL 4: BLUE ARM DAC RE-FRESH JOB, FORCE SENSING ERROR MESSAGE RTN
;YELLOW INTERFACE DEFS
DRATRP ==300 ;yellow arm DR11C A vector -- A/D conversion done
DRBTRP ==304 ;yellow arm DR11C B vector -- Panic or timeout int.
DR11S =167770 ;DR11 STATUS WORD
DR11O =167772 ;DR11 OUTPUT REGISTER
DR11I =167774 ;DR11 INPUT REGISTER
PANICB =200
YDACEN =100000 ;YELLOW DAC ENABLE BIT
EMFSEN =200
;BLUE INTERFACE DEFS
DACCSR =174000 ;DAC STATUS AND CONTROL WORD ADDRESS
ADCSR =174004 ;ADC CONTROL AND STATUS WORD ADDRESS
ADCCHN =174005 ;ADC CHANNEL ADDRESS
ADCVAL =174006 ;ADC VALUE ADDRESS
DACDNE ==200 ;DAC DONE BIT
;RANDOM DEFINITIONS
DBUFL ==6000. ;LENGTH OF DIAGNOSTIC BUFFER
MAXSRV ==20. ;NUMBER OF EXISTING SERVOS
NULTME ==5000. ;NUMBER OF MILLISECONDS ALLOWED TO NULL FINAL POS. ERROR
STPTME ==1000. ; " " " " FOR ARM TO STOP AFTER AN
; ERROR HAS OCCURRED
CTRSTP ==1500. ;NUMBER OF MILLISECONDS ALLOWED FOR HAND TO CLOSE AFTER
; BOTH TOUCH SENSORS ACTIVATED IN CENTER FUNCTION
;REFERENCE POWER SUPPLY DATA
DATA
REFCN1 ==16 ;ADC CHANNELS FOR REFERENCE READING
REFCN2 ==17
REFER1: -1007. ;POWER SUPPLY READING AT TIME OF JOINT CALIBRATION
REFER2: 1021.
REFVT1: -1007. ;CURRENT POWER SUPPLY READINGS
REFVT2: 1021.
REFTOL ==6 ;ERROR TOLERANCE PERMITTED BEFORE "BAD REF." IS SIGNALLED
;PUMA definitions:
NJTS == 6 ;Number of joints the arm has
;SPECIAL JOINT 7 OUTPUT BITS
IOREG == 7 ;JOINT 7 IS THE INPUT/OUTPUT CONTROL REGISTER
ENABLE == 400 ;HIGH POWER TO JOINT SERVOS
AIROPN == 1000 ;TURNS ON AIR PRESSURE IN PNEUMATIC TUBE TO OPEN HAND
AIRCLS == 2000 ;CHANGES PRESSURE TO CLOSE HAND
;SPECIAL JOINT 7 INPUT BITS
ISON == 400 ;HIGH POWER ON (LOW IF OFF)
;ARM INTERFACE CONTROL WORD BIT DEFINITIONS
POSMDE == 0 ;MOVE JOINT IN POSITION MODE
CURRNT == 10 ;CURRENT RATHER THAN POSITION MODE
STOLRG == 20 ;SET (POSITION) TOLERANCE RANGE
SETPOS == 30 ;SET POSITION
SINTRG == 60 ;SET INTEGRATION RANGE
SMODE == 70 ;WRITE A BYTE TO THE JOINT SERVO'S MEMORY (USUALLY STATUS)
STOPCM == 100 ;STOP JOINT MOTION.
RDPOS == 140 ;READ POSITION COUNTERS
RDSTAT == 150 ;SERVO STATUS WORD (TO READ)
READC == 160 ;READ ADC (POTENTIOMETER)
SEQMDE == 200 ;SEQUENTIAL ACCESS MODE BIT IN COMMAND FOR MICRO
;ERROR MESSAGE OFFSET MASKS:
BADPOT == 1000 ;OFFSET 02 INTO TABLE = BAD POT
NOZIND == 2000 ;OFFSET 04 = NO ZERO INDEX
ADCDEA == 3000 ;OFFSET 06 = ADC DEAD
;EXJTFC == 4000 ;ALREADY DEFINED ABOVE. OFFSET 10 = EXCESSIVE JOINT FORCE.
SRVDEA == 5000 ;OFFSET 12 = SERVO DEAD
PASLIM == 20000 ;ALREADY DEFINED ABOVE - PAST LIMIT
;RELATIVE ADDRESSES FROM START OF PUMA DATA BLOCK
;the below definitions are used in place of DELTH, DELTHD and POSERR in a
;conventional data block because the format of a puma data block is different.
PJT1 == 10. ;PTR TO DESIRED PUMA JOINT ANGLE 1
PJT2 == 14. ; " " " " 2
PJT3 == 18. ; " " " " 3
PJT4 == 22. ; " " " " 4
PJT5 == 26. ; " " " " 5
PJT6 == 30. ; " " " " 6
PTH == PJT1 ;Ptr to PUMA joint angle table
PPOLY1 == 34. ;PTR TO A5 POLYNOMIAL FOR JOINT 1
PPOLY2 == 36. ; " " " " 2
PPOLY3 == 38. ; " " " " 3
PPOLY4 == 40. ; " " " " 4
PPOLY5 == 42. ; " " " " 5
PPOLY6 == 44. ; " " " " 6
;MACROS for establishing relative pointers for DEVICE DATA BLOCK
;INT ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 2
.MACRO INT SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+2
.ENDM
;BYTE ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 1
.MACRO BYTE SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+1
.ENDM
;REAL ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 4
.MACRO REAL SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+4
.ENDM
;SIXVEC ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 2*6, TO ALLOW FOR A 6-WORD VECTOR.
.MACRO SIXVEC SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+12.
.ENDM
;F6VEC ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 4*6, TO ALLOW FOR A 6-WORD VECTOR OF FLOATING-PT NUMBERS.
.MACRO F6VEC SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+24.
.ENDM
;LSKIP INCREMENTS THE RELATIVE POINTER "RELCNT" BY 2*N
.MACRO LSKIP N
.REPT N
RELCNT == RELCNT+2
.ENDR
.ENDM
;MACROS to establish pointer tables into arm and hardware device SERVO DATA BLOCKS
.MACRO YELPTR LABEL
YJTPTR LABEL
SRV07+LABEL
.ENDM
.MACRO YJTPTR LABEL
SRV01+LABEL
SRV02+LABEL
SRV03+LABEL
SRV04+LABEL
SRV05+LABEL
SRV06+LABEL
.ENDM
.MACRO BLUPTR LABEL
BJTPTR LABEL
SRV14+LABEL
.ENDM
.MACRO BJTPTR LABEL
SRV08+LABEL
SRV09+LABEL
SRV10+LABEL
SRV11+LABEL
SRV12+LABEL
SRV13+LABEL
.ENDM
.MACRO HDVPTR LABEL
SRV15+LABEL
SRV16+LABEL
.ENDM
.macro grnptr label
srv17+label
srv18+label
.endm
.macro grnspf label ;grnspf is a special format where the
srv17+label ;next pointer follows the previous one
srv17+label+4 ;this is because the puma data block's
srv17+label+8. ;joint angle and poly pointers are right
srv17+label+12. ;after one another.
srv17+label+16.
srv17+label+20.
srv18+label
.endm
.macro redptr label
srv19+label
srv20+label
.endm
.macro redspf label ;redspf is a special format where the
srv19+label ;next pointer follows the previous one.
srv19+label+4 ;This is because the puma data block's
srv19+label+8. ;joint angle and poly pointers are right
srv19+label+12. ;after one another.
srv19+label+16.
srv19+label+20.
srv20+label
.endm
;Pointer tables for servo data
DATA
SERVOS: ;POINTERS TO SERVO LISTS, ORDER FOLLOWS BIT ASSIGNMENTS
YSRVOS: YELPTR 0 ;YELLOW ARM AND HAND JOINTS
BSRVOS: BLUPTR 0 ;BLUE ARM AND HAND JOINTS
HDEVS: HDVPTR 0 ;POINTER TO HARDWARE DEVICES: VISE AND SCREW DRIVER
GSRVOS: grnptr 0 ;pointer to green arm and hand joints
RSRVOS: redptr 0 ;pointer to red arm and hand joints
;POINTERS TO ARM JOINT ANGLES
THPTR:
YTH: YELPTR TH ;POINTERS TO CURRENT YELLOW ARM JOINT ANGLES
BTH: BLUPTR TH ; " " " BLUE " " "
HTH: HDVPTR TH ; " " " HARDWARE DEVICE JOINT ANGLES
gth: grnspf pth ;pointers to current green arm joint angles
rth: redspf pth ; " " red " " "
THPPTR:
YTHP: YELPTR THP ;POINTERS TO PREDICTED YELLOW ARM JOINT ANGLES
BTHP: BLUPTR THP ; " " " BLUE " " "
;POINTERS TO RAW POT READINGS
POTPTR: YELPTR POT
BLUPTR POT
;TABLE OF POINTERS TO YELLOW ARM DYNAMIC COEFFICIENTS
YCI: YJTPTR CI ;CURRENT GRAVITY LOADING TERMS
YJTPTR CII ; " JOINT INERTIA TERMS
YNCI: YJTPTR NCI ;INITIAL GRAVITY LOADING TERMS
YJTPTR NCII ; " JOINT INERTIA TERMS
;TABLE OF POINTERS TO BLUE ARM DYNAMIC COEFFICIENTS, CI & CII
BCI: BJTPTR CI ;CURRENT GRAVITY LOADING TERMS
BJTPTR CII ; " JOINT INERTIA TERMS
BNCI: BJTPTR NCI ;INITIAL GRAVITY LOADING TERMS
BJTPTR NCII ; " JOINT INERTIA TERMS
;TABLE OF POINTERS TO ERROR TORQUE TERMS
ERRPTR: YELPTR ERRTQE ;POINTERS TO JOINT ERROR TORQUE TERMS
BLUPTR ERRTQE ; " " " " " "
;TABLE OF POINTERS TO SERVO DEVICE BLOCKS
DVCPTR: YELPTR INUSE ;POINTERS TO DEVICES TO WHICH SERVOS ARE ATTACHED
BLUPTR INUSE
HDVPTR INUSE
grnptr inuse
redptr inuse
;TABLE OF POINTERS TO JOINT POSITION ERROR TERMS
PERPTR: BJTPTR POSERR ;POINTERS TO POSITION ERROR TERMS
VERPTR: BJTPTR VELERR ;POINTERS TO VELOCITY ERROR TERMS
;Servo function error codes & bit masks for servo, status ans mode words
;THE FOLLOWING ARE THE POSSIBLE ERROR CODES THAT CAN BE RETURNED IN R0 AFTER
;A SERVO FUNCTION HAS BEEN EXECUTED, EG. MOVE,CENTER.
CNTATT==1 ;COULD NOT ATTACH TO REQUESTED JOINT(S)
WRGNUM==2 ;INCORRECT NUMBER OF JOINTS REQUESTED TO BE DRIVEN
BADWIP==3 ;WIPERS COULD NOT BE READ WITHIN THEIR OPERATING RANGE
NOSOLU==4 ;ARM SOLUTION DOES NOT EXIST
UNKTCH==5 ;UNKNOWN TOUCH SENSOR REQUESTED
TOMTCH==6 ;NO MORE FREE SLOTS IN TOUCH SENSOR EVENT LIST
NOPOWR==7 ;ARM INTERFACE POWER SUPPLY TURNED OFF
BADREF==10 ;REFERENCE POWER SUPPLY OUT OF RANGE
BADTAC==11 ;ZERO VELOCITY TACHOMETER READING OUT OF RANGE
WRGARM==12 ;ATTEMPTED TO SWITCH ARMS WHILE FORCE SERVOING
TOMFRC==13 ;NO MORE FREE SLOTS IN FORCE SENSOR EVENT LIST
FWRGJT==14 ;NEED ALL 6 ARM JOINTS IN ORDER TO DO FORCE SENSING/COMPLIANCE
NOPOLY==15 ;CAN'T FORCE SERVO MOTION WITHOUT POLYNOMIAL
BOVERR==16 ;BACKGROUND JOB TOOK TOO LONG TO EXECUTE
TOMSRV==17 ;TOO MUCH SERVOS REQUESTED TO RUN
;IF NO ERROR OCCURRED, A ZERO IS RETURNED. ANY OTHER NON-ZERO R0 VALUES
;CORRESPONDS TO THE STATUS WORD BIT ERROR FLAGS PRESENTED BELOW, EG. NONEX,
;ADERR.
;STATUS WORD BIT MASKS
NXTFIN ==1 ;NEXT SERVICING PERIOD WILL BE "FINAL".
FINAL ==2 ;IN FINAL STAGE OF MOTION, JUST NULLING ERRORS
;!!!!!!DONT CHANGE BIT ASSIGNMENTS OF "NXTFIN"&"FINAL"!!!!!!
RUN ==4 ;RUN SERVO, SET TO ZERO TO STOP SERVOING
MOVING ==10 ;DEVICE IS IN MOTION, I.E. THE VELOCITY IS NOT KNOWN TO BE ZERO
STROUT ==20 ;JOINT STARTED OUTSIDE OF PERMITTED OPERATING RANGE
FIRST ==40 ;FIRST PASS THRU SERVO
FSERVO ==100 ;FORCE SERVO THIS JOINT
srvkil ==200 ;killed servo due to all motion in servo finished
NONEX ==400 ;JOINT IS DOWN, INOPERABLE
ADERR ==1000 ;CATASTROPHIC A/D ERROR HAS OCCURRED
PANCBT ==2000 ;PANIC BUTTON WAS PUSHED
EXJTFC ==4000 ;EXCESSIVE JOINT FORCE ERROR
TMEOUT ==10000 ;FUNCTION TOOK TOO LONG TO EXECUTE
PASLIM ==20000 ;JOINT STOPPED BEYOND STOP LIMIT
FNOSOL ==40000 ;NO ARM SOLUTION WHILE DOING FORCE COMPLIANCE
;MODE WORD BIT MASKS
NNUL ==1 ;DO NOT NULL POSITION ERROR AT THE END OF A TRAJECTORY
WOBBLE ==2 ;WOBBLE OUTER JOINTS AT END OF TRAJECTORY
DEPTPT ==4 ;MOTION HAS A DEPARTURE POINT
;BIT MASKS TO TEST FOR BLUE OR YELLOW HAND IN POLYNOMIAL BLOCK HEADER
YHAND ==1000 ;YELLOW HAND
BHAND ==4 ;BLUE HAND
ghand ==40000 ;green PUMA hand (2nd word)
rhand ==10000 ;red PUMA hand (2nd word)
;BIT MASKS TO TEST FOR BLUE OR YELLOW ARM JOINTS IN POLYNOMIAL BLOCK HEADER
YARM ==176000;YELLOW ARM
BARM ==770 ;BLUE ARM
garm ==100000;green PUMA arm (2nd word)
rarm ==20000 ;red PUMA arm (2nd word)
;BIT MASKS TO TEST FOR VISE AND SCREWDRIVER IN POLYNOMIAL BLOCK HEADER
VISE ==2 ;VISE
SCREW ==1 ;SCREWDRIVER
;Organization of SERVO DATA BLOCK
;RELATIVE ADDRESSES FROM START OF conventional SERVO DATA BLOCK
;PUMA arm data blocks differ in that JOINT1-6, PPOLY1-6 is substituted
;instead of TH - POSERR.
RELCNT ==2 ;FIRST WORD CONTAINS STATUS BITS
INT MODBTS ;OPERATION MODE BITS, EG. WOBBLE, NNUL
INT INUSE ;0 IF SERVO NOT ATTACHED, ELSE POINTER TO DEVICE BLOCK.
INT SRVNUM ;LOGICAL NUMBER OF SERVO
INT MECHSM ;BIT INDICATING WHICH MECHANISM THIS SERVO IS A PART OF(SEE BELOW)
REAL TH ;CURRENT JOINT ANGLE READ FROM POTS IN DEGREES
REAL THP ;PREDICTED JOINT ANGLE IN DEGREES
REAL THN ;PREDICTED JOINT ANGLE FOR NEXT SERVOING PERIOD, IN DEGREES
REAL THD ;CURRENT ANGULAR VELOCITY OF JOINT IN DEGREES/TICK
REAL THDP ;PREDICTED ANGULAR VELOCITY IN DEGREES/TICK
REAL THDN ;PREDICTED ANGULAR VELOCITY FOR NEXT SERVOING PERIOD, IN DEGREES
REAL DELTH ;TOTAL DEVIATION OF JOINT ANGLE DUE TO DELTHD
REAL DELTHD ;VELOCITY FACTOR FOR MODIFYING TRAJECTORY WHILE IN MOTION
REAL POSERR ;POSITION ERROR, TH-THP
INT WOBPTR ;INDEX INTO WOBBLE SINUSOIDAL TABLE, -1 FOR JTS THAT DONT WOB.
INT WOBMAG ;PTR TO MAGNITUDE OF WOBBLE IN JTS 4,5,6
INT DATPT ;PTR TO START OF POLYNOMIAL DATA LIST
INT POLY ;PTR TO POLYNOMIAL A5 COEF. FOR THIS SERVO
INT FCI ;PTR TO FINAL JOINT GRAVITY LOADING FOR SEGMENT
INT TTIME ;TOTAL TIME OF PRESENT SEGMENT, IN TICKS
REAL FTTIME ; " " " " " " "
INT PTIME ;TIME INTO THE PRESENT SEGMENT WHEN WE SERVICE AGAIN, IN TICKS
;Following data is not found in PUMA data blocks, only in Stanford arms
;and DRIVER and VISE.
REAL ELTIME ;ELAPSED TIME SINCE LAST READING IN TICKS
INT COUNT ;TIME ALLOWED FOR PRESENT FUNCTION
REAL ERRINT ;INTEGRAL OF THE POSITION ERROR
REAL ERRTQE ;TOTAL ERROR TORQUE TERM
REAL ERRTOL ;ERROR TOLERANCE FOR DETERMINING IF POS., VEL. WITHIN LIMITS
REAL KV ;VELOCITY ERROR FEEDBACK RATIO (NEG #)
REAL KE ;POSITION ERROR FEEDBACK RATIO (NEG #)
REAL KI ;INTEGRAL POSITION ERROR FEEDBACK RATIO (NEG #)
REAL V0 ;FRICTION RESISTANCE IN MOTOR TORQUE
REAL CI ;CURRENT JOINT GRAVITY LOADING TERM
REAL DCI ;CHANGE IN JOINT GRAVITY LOADING THROUGH SEGMENT
REAL NCI ;INITIAL GRAVITY LOADING AT START OF SEGMENT
REAL CII ;CURRENT JOINT INERTIA TERM
REAL DCII ;CHANGE IN JOINT INERTIA THROUGH SEGMENT
REAL NCII ;INITIAL JOINT INERTIA AT START OF SEGMENT
REAL FLEVEL ;FORCE LEVEL OUTPUT IF IN FORCE SERVO MODE
REAL KKI ;INTEGRAL FEEDBACK FOR STIFFNESS SYSTEM
REAL KKV ;VELOCITY FEEDBACK FOR STIFFNESS SYSTEM
REAL VELERR ;VELOCITY ERROR, THD-THDP
REAL KKK ;STIFFNESS IN THIS JOINT OZ-IN/DEG
REAL KKF ;FORCE ERROR FEEDBACK COEFFICENT FOR STIFFNESS SYSTEM
REAL YOLD ;FORCE COMPENSATION OLD OUTPUT
REAL UOLD ;FORCE COMPENSATION OLD INPUT
INT POT ;A/D POSITION POT READING, UNMODIFIED
INT TACH ;A/D TACHOMETER READINGS, UNMODIFIED
INT POTCHN ;A/D POSITION POT CHANNEL
INT TACCHN ;A/D TACHOMETER CHANNEL, -1 IF NONE
INT DACADD ;DAC STATUS/CONTROL WORD ADDRESS
BYTE DACCHN ;DAC CHANNEL FOR MOTOR + TIME OUT RESTART
BYTE DACVAL ;OUTPUT TO DAC SAMPLE WORD
INT DACINF ;DAC TACH. FEEDBACK BIT IN LEFT HALF, BRAKE BIT IN RIGHT HALF
REAL MSCALE ;SCALE FACTOR TO CONVERT MOTOR TORQUE TO DAC UNITS
REAL TOTQE ;CONVERSION FACTOR FROM DEG. TO RAD. FOR PROPER UNITS OF TORQUE
INT DITHER ;MAGNITUDE OF DITHER TO BE ADDED TO JOINT TORQUE IN DAC UNITS
REAL SCALE ;SCALE FACTOR TO CONVERT FROM A/D UNITS TO DEGREES
REAL OFFSET ;OFFSET OF A/D POT READING
REAL USTOP ;UPPER LIMIT ON JOINT MOTION IN DEGREES
REAL LSTOP ;LOWER " " " " " "
REAL STPBND ;WIDTH OF STOP BAND BETWEEN USTOP OR LSTOP AND PHYSICAL STOP
REAL VSCALE ;TACH CONVERSION FACTOR FROM A/D UNITS TO DEGREES/TICK
INT MAXDRV ;MAXIMUM PERMITTED MOTOR DRIVE TORQUE
INT TACHZ0 ;A/D TACH READING AT ZERO VELOCITY
INT WIPERS ;POINTER TO MULTIPLE WIPER DATA, ZERO IF ONLY ONE WIPER
.IFZ ISLIN
INT NONLIN ;START OF 18 BYTES OF NON-LINEAR CALIBRATION DATA FOR POT
LSKIP 10
.ENDC
;THE MULTIPLE WIPER TABLE FOLLOWS. THE FORMAT for this table is listed
;ON THE PAGE WITH THE "WIPERS" SUBROUTINE.
;Here is the rest of the table for the PUMA data blocks:
RELCNT == ELTIME ;Reset counter to next available spot
INT BADPJT ;Bits indicating which joint(s) are in error
INT DRADDR ;Address of the DR11C for this arm.
INT ANGADR ;Address of angle vector (e.g. GTH or RTH) for this arm.
INT PNAME ;Name of the puma (red/green) in ASCIZ format (3 words)
LSKIP 2
SIXVEC PWORK6 ;For 6 encoder counts, etc.
SIXVEC JANGLE ;WORK ANGLES OR ANYTHING ELSE
SIXVEC LASTTH ;LAST ANGLES PUMA MOVED TO.
INT ARMBIT ;WHICH BITS ARE ON IN THIS ARM'S INTERFACE (POWER,HAND,ETC)
INT CALFLG ;0=ARM NOT CALIBRATED YET.
INT CTIME ;WHERE WE ARE IN CALIBRATION STAGE
INT SRVERR ;WHICH JOINT IS IN ERROR
F6VEC EFIRST ;ANGLE AT FIRST ENCODER ZERO INDEX
F6VEC ADCSCL ;ADC SCALE FACTOR, IN DEGREES/ADC COUNT
F6VEC ADCOFS ;OFFSET (INTERCEPT) OF ADC, IN DEGREES.
;MECHANISM BIT FLAGS SET IN THE "MECHSM" BYTE VARIABLE
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
YELHND ==2 ;YELLOW HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
BLUHND ==10 ;BLUE HAND
VIS ==20 ;VISE
SCRE ==40 ;SCREW DRIVER
grnarm ==100 ;green PUMA arm, not including hand
grnhnd ==200 ;green hand
redarm ==400 ;red PUMA arm, not including hand
redhnd ==1000 ;red hand
;Data list structures for polynomial coefficients and DEVICE BLOCKS
;
;THE FOLLOWING DESCRIBES THE REQUIRED ORGANIZATION FOR THE POLYNOMIAL
;COEFFICIENTS DATA LIST AND ASSOCIATED SERVO POINTERS:
;
; SERVO POINTERS DATA ARRAY
;
; XXXXXX TWO SERVO BIT WORDS
; XXXXXX 1 BIT FOR EACH SERVO
; BITS COMMAND MODE BITS, EG. NNUL,WOBBLE
; WOBMAG PTR TO WOBBLE MAGNITUDE CELL
; DATPT{PTR} → SEG2-. REL.POINTER TO THE NEXT SEGMENT
; TIME LENGTH OF SEGMENT IN MILLISEC
; TRANS POINTER TO LIST OF END POINT
; TRANSFORMS AND VALIDITY NUMBERS
; CODE PTR TO CODE TO BE SCHEDULED THIS SEG
; A0 F.P. POLY. COEF. FOR FIRST SERVO
; :
; A5
; :
; :
; A0
; :
; POLY{PTR}→ A5 F.P. POLY. COEF. FOR THIS SERVO
; :
; :
; A0
; :
; :
; A5
; NCI FINAL JT. GRAV. LOADING FOR 1ST JT.
; NCII FINAL JT. INERTIA FOR 1ST JOINT
; :
; FCI{PTR}→ NCI FINAL JT. GRAV. LD. FOR THIS SERVO
; NCII FINAL JT. INERTIA
; :
; SEG2: SEG3-. START OF NEXT SEGMENT
; :
; SEGN: 0 DUMMY SEGMENT, THIS SIGNALS THE
; END OF SEGMENT DATA
;
;THE NCI AND NCII TERMS ARE FILLED IN BY THE PRE-MOTION TRAJECTORY MODIFIER.
;SPACE FOR THESE TERMS MUST BE ALLOCATED BY THE CALLING INTERPRETER.
OPBITS ==4 ;RELATIVE ADDRESS OF MODE BITS, SEE PAGE 8 FOR BIT ASSIGNMENTS
WOBMG ==6 ;PTR TO WOBBLE MAGNITUDE CELL
DATST ==10 ;RELATIVE ADDRESS OF START OF SEGMENT DATA
;RELATIVE ADDRESSES OF DATA POINTED TO BY "DATPT"
;FIRST WORD CONTAINS RELATIVE POINTER TO NEXT TRAJECTORY LIST
SEGTME ==2 ;LENGTH OF TRAJECTORY SEGMENT IN TICKS
SEGTRN ==4 ;POINTS TO LIST OF END POINT TRANSFORMATIONS AND VALIDITY NUMBERS
; SEE STRUCTION OF LIST BELOW.
RNCODE ==6 ;POINTER TO CODE THAT IS TO BE SCHEDULED AT END OF SEGMENT
A0COEF ==10 ;CONSTANT TERM OF FIRST POLYNOMIAL
A3COEF ==24 ;A3 TERM OF FIRST POLYNOMIAL
A5COEF ==34 ;A5 " " " "
;TRANSFORM/VALIDITY LIST STRUCTURE:
;
; TRANS→ PTR TO VARIABLE 1 ;ARGUMENT GIVEN TO "GETARG"&"GETVAL"
; VALIDITY NUMBER ;# RETURNED BY TRANSFORM LAST TIME
; ; IT WAS INTERROGATED.
; PTR TO VARIABLE N
; VALIDITY NUMBER
;DEVICE BLOCK ORGANIZATION AND ASSOCIATED SERVO POINTER
;
; INUSE→ DEVICE: #NUM,0 ;NUMBER OF SERVOS STILL ACTIVE
; #EVENT ;EVENT TO BE SIGNALED WHEN ALL SERVOS DONE
; SRV01 ;LIST OF ATTACHED SERVOS
; SRV02
; :
; SRVXX
; 0 ;END OF LIST
;
;THE LOWER BYTE OF THE FIRST WORD OF THE DEVICE BLOCK IS USED TO STORE THE
;NUMBER OF SERVOS ATTACHED. THE HIGH BYTE IS USED TO SIGNAL ERROR CONDITIONS
;WHEN THE SERVOS ARE RUNNING.
;SERVO - level 6 servo code
CODE
;The only argument required by "SERVO" is a pointer to the servo device status
;array for the devices that are to be serviced. During 1 servo period
;all devices are serviced in the order they appear in the device status array.
;After the devices have been serviced, other level 6 tasks are checked to see
;if they need to be run in the remaining period. (force sensing, etc.)
;SERVO ALWAYS RUNS EACH 16 MS.
;
;REGISTERS USED:
;
; ALL CPU AND FLOATING POINT REGISTERS GARBAGED
servo: mov #DVSTAT,dvarrp ;get device status array pointer
mov #16.,schtim ;assume scheduled time for next run ( ?? JJC )
dvloop: mov @dvarrp,devpnt ;get pointer to device block
beq nxtdev ;branch if this entry is not an active device
cmp #-1,devpnt ;did we reach the end of the status array?
beq chklv6
mov devpnt,cursrv ;set up the current servo pointer for SVLOOP routine
tstb @cursrv ;are all servos finished?
bne 1$ ;not finished yet, so must go run the servos
;if the number of active servos in the device block is zero, this means
;that the device is completely done so we must signal that event.
add #2,cursrv ;point to event number
EVSIG @cursrv ;SIGNAL EVENT
clr @dvarrp ;clear entry in device status array to indicate
;that this device is now inactive
br nxtdev
1$: add #4,cursrv ;point to 1st servo in device block
cmp dvarrp,#garmdv ;if its a puma arm, call a different driver routine
blo 2$
jsr pc,puma ;go run the puma arm
br nxtdev
2$: bit #run,stfdat ;are we stiffness servoing?
beq 3$ ;br if not
mov @cursrv,dat ;else check if blue arm
bit #bluarm,mechsm(dat)
beq 3$ ;br if not
jsr pc,ksrvo ;else do stiffness servo job
br nxtdev ;cant center if stiffness servoing
3$: jsr pc,svloop ;go run the servos found in the device block
mov devpnt,dat ;check if a centering operation is in progress
jsr pc,CTRLV6
nxtdev: add #2,dvarrp ;bump pointer to next device entry in status array
br dvloop
;finished servicing all the devices, check FOR OTHER LEVEL 6 ROUTINES to be run
CHKLV6: TST GOSTH ;SETTH LVL 6 CODE NEED TO BE RUN?
BEQ 1$
JSR PC,STHLV6
1$: TST GOSRF ;SETRF LVL 6 CODE NEED TO BE RUN?
BEQ 2$
JSR PC,SRFLV6
2$: TST GOOPR ;OPERATE LVL 6 CODE?
BEQ 3$
JSR PC,OPRLV6
3$: TST GOBAS ;
BEQ 4$
JSR PC,BASLV6
4$: TST GOWST ;
BEQ FINSTP
JSR PC,WSTLV6
FINSTP: tst barmdv ;is blue arm active?
bne 2$
tst bhandv ;is blue hand active?
beq srvdis ;br if yellow arm
2$: tst gdata ;is there gathering requested?
bne 3$
bit #run,fstat ;need to run force sensing or servoing?
beq srvdis
3$: mov #xfptr,dat
jsr pc,frclv6 ;GO DO FORCE STUFF
srvdis: TST FINFLG ;HAS FINARM BEEN CALLED BY AL?
BEQ 1$ ;NO, SERVO STAYS ALIVE BUT SLEEPS FOR A BIT
DISMIS ;END OF RUN, KILL SERVO JOB
1$: SLEEP #16. ;SLEEP TIL NEXT TICK
JMP SERVO
DATA
;LEVEL 6 SERVICE REQUEST TABLE
GOSTH: .WORD 0 ;SETTH
GOSRF: .WORD 0 ;SETREF
GOOPR: .WORD 0 ;OPERATE
GOBAS: .WORD 0 ;
GOWST: .WORD 0 ;
CODE
; SVLOOP - subroutine of "servo"
;The pointer CURSRV must be pointing to the first servo to be run in the
;device block. SVLOOP will examine and run each servo (if appropriate) in a
;sequential manner in the order specified in the device block.
;
;THE "RUN" AND "MOVING" BITS IN THE SERVO STATUS WORD ARE USED TO DECIDE
;WHAT IS TO BE DONE for each servo. THE POSSIBLE COMBINATIONS OF RUN AND MOVING
;AND THEIR INTERPRETATIONS ARE PRESENTED BELOW:
;
; RUN MOVING INTERPRETATION
; 0 0 NO WORK TO DO, THE SERVO KILLS ITSELF.
; 0 1 STOPPING MOTION, SERVO CONTINUES TO run until the joint
; VELOCITY IS BELOW THE ERROR TOLERANCE
; 1 0 MOTION NOT YET STARTED, SERVO CONTINUES TO wait
; AND DO NOTHING UNTIL THE START WAIT COUNT {TTIME} IN
; MILLISECONDS IS LESS THAN OR EQUAL TO "PTIME".
; 1 1 JOINT RUNNING, EXECUTE SERVO CODE.
;
;THE SERVO WILL BEGIN ITS EXITING SEQUENCE IF ANY OF THE FOLLOWING EVENTS TAKES
;PLACE:
; 1. THE MOTION IS COMPLETED AND THE VELOCITY AND POSITION ERRORS HAVE BEEN
; NULLED TO WITHIN TOLERANCE.
; 2. THE VARIABLE "COUNT" HAS BEEN DECREMENTED PAST ZERO, IN WHICH CASE THE
; "TMEOUT" FLAG IS SET AND ALL OTHER SERVOS ARE SIGNALED TO STOP.
; 3. THE VARIABLE "BPANIC" IS FOUND TO BE NON-ZERO INDICATING THAT THE
; PANIC BUTTON HAS BEEN HIT.
; 4. THE SIGN BIT OF THE FIRST WORD OF THE DEVICE BLOCK IS SET INDICATING
; THAT ANOTHER SERVO HAS HAD A CATASTROPHIC ERROR OCCUR.
;
;IF AN ERROR OCCURRED WHILE THE JOINT WAS IN MOTION, THE BRAKES ARE SET AND A
;SPECIFIED AMOUNT OF TIME IS ALLOWED FOR THE JOINT VELOCITY TO GO TO ZERO
;BEFORE THE SERVO SIGNALS THE OTHER SERVOS ATTACHED TO THE DEVICE THAT IT HAS
;COMPLETED ITS MOTION.
;
svloop: mov @cursrv,dat ;dat points to servo data block being serviced
beq srvrtn ;we are done serviceing all the servos, return
add #2,cursrv ;bump pointer for next time through this loop
bit #srvkil,(dat) ;has this servo been killed?
bne svloop ;yes, go to next servo
;check if function has timed out
SUB #SDTIME,COUNT(DAT);DECREMENT TIME COUNT BY MIN. SAMP. INTEV.
BGT 1$ ;BRANCH IF MORE TIME LEFT
BIT #TMEOUT,(DAT) ;ARE WE STUCK IN A TIMEOUT LOOP?
BEQ .+6
BIC #MOVING,(DAT) ;YES, DONT WAIT FOR ZERO VELOCITY, JUST DIE
BIS #TMEOUT,(DAT) ;SET FUNCTION TOOK TOO LONG FLAG
JSR PC,CATERR ;SIGNAL OTHER SERVOS OF CATASTROPHIC ERROR
BR CHKSTP
;CHECK IF STOPPING OR IF ANOTHER JOINT HAS SUFFERED A FATAL ERROR
1$: BIT #RUN,(DAT) ;CHECK IF JOINT IN THE PROCESS OF STOPPING
BEQ CHKSTP ;BRANCH IF STOPPING
TST @INUSE(DAT) ;ELSE CHECK IF SOMEONE ELSE SIGNALED A CATAS. ERROR
BPL 2$ ;BRANCH IF NO ERROR SIGNALED
JSR PC,STPMVE ;ELSE SET THE BRAKES AND START EXIT SEQUENCE
BR CHKSTP
;CHECK IF THE PANIC BUTTON WAS HIT
2$: TST BPANIC ;CHECK IF THE PANIC BUTTON HAS BEEN HIT
BEQ STWAIT ;BRANCH IF OK
BIS #PANCBT,(DAT) ;ELSE INDICATE PANIC BUTTON HIT
JSR PC,CATERR ;SIGNAL OTHER JOINTS, CATASTROPHIC ERROR
;AFTER AN ERROR OCCURRED, CHECK IF JOINT MOTION STOPPED
CHKSTP: JSR PC,MOTSTP ;MAKE SURE BRAKES ARE ON
JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
BIT #MOVING,(DAT) ;CHECK IF SERVOING COMPLETED
BEQ SRVDNE ;KILL SERVO IF "MOVING" AND "RUN" ARE OFF
ABSF AC1 ;GET VELOCITY MAGNITUDE
CMPF ERRTOL(DAT),AC1 ;CHECK STOPPED, I.E. VEL LESS THAN ERR TOLERANCE
CFCC
BLT 1$ ;SKIP IF JOINT STILL MOVING
BIC #MOVING,(DAT) ;ELSE SIGNAL MOTION STOPPED
BR SRVDNE ;GO KILL SERVO
1$: ;update ptime and eltime so that they are current when servo wakes up again.
LDCIF schtim,AC2 ;GET ACTUAL TIME TILL NEXT RUN
ADD schtim,PTIME(DAT) ;KEEP TRACK OF TIME INTO PRESENT SEGMENT
STF AC2,ELTIME(DAT) ;SAVE F.P. ELAPSED TIME
JSR PC,WIPER ;CHECK IF POT WIPER IN RANGE
br svloop ;still moving, check again next time
;IF START WAIT STATE, COMPARE DEAD BAND TIME "TTIME" TO PRESENT TIME "PTIME"
STWAIT: BIT #MOVING,(DAT) ;CHECK IF JOINT IN MOTION
BNE SERVNG ;IF IN MOTION, GO SERVO THE JOINT
CMP TTIME(DAT),PTIME(DAT) ;ELSE MUST BE IN START WAIT STATE,
; CHECK IF TIME TO START SERVOING
BLE 1$ ;IF PTIME≥TTIME, GO SERVO
;update ptime and eltime so that they are current when servo wakes up again.
LDCIF schtim,AC2 ;GET ACTUAL TIME TILL NEXT RUN
ADD schtim,PTIME(DAT) ;KEEP TRACK OF TIME INTO PRESENT SEGMENT
STF AC2,ELTIME(DAT) ;SAVE F.P. ELAPSED TIME
br svloop ;do nothing, check again next time
1$: BIS #MOVING+FIRST,(DAT) ;INDICATE JOINT IN MOTION AND FIRST PASS
; THRU SERVO CODE
SUB TTIME(DAT),PTIME(DAT) ;CORRECT CURRENT TIME FOR DEAD BAND
JSR PC,BRKREL ;RELEASE THE JOINT BRAKE
BR FSTSRV
;WE ARE ACTUALLY SERVOING, CHECK IF WE NEED TO EVALUATE A POLYNOMIAL
SERVNG: LDF THN(DAT),AC0 ;UPDATE PREDICTED JOINT ANGLE
STF AC0,THP(DAT)
LDF THDN(DAT),AC0 ;UPDATE PREDICTED VELOCITY
STF AC0,THDP(DAT)
BIT #FINAL,(DAT) ;CHECK IF JUST NULLING ERRORS
BNE SAMP ;BRANCH IF SO
;update ptime and eltime so that they are current when servo wakes up again.
fstsrv: LDCIF schtim,AC2 ;GET ACTUAL TIME TILL NEXT RUN
ADD schtim,PTIME(DAT) ;KEEP TRACK OF TIME INTO PRESENT SEGMENT
STF AC2,ELTIME(DAT) ;SAVE F.P. ELAPSED TIME
JSR PC,EVAL ;EVALATE TRAJECTORY POLYNOMIAL AND COMPUTE
; INERTIAL TORQUE
;SAMPLE THE A/D TO UPDATE THE CURRENT POSITION, "TH", AND VELOCITY, "THD",
;AND COMPUTE FEEDBACK CORRECTION
SAMP: JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
JSR PC,FEEDBK ;COMPUTE ERROR FEEDBACK TERMS
JSR PC,WIPER ;GET PROPER WIPER FOR NEXT SERVICING PERIOD
.IFNZ DIAGY
BR SAVDAT
SDRET:
.ENDC
.IFNZ TIMER
MOVB CLKCNT,@TIMES ;SAVE EXECUTION TIME
INC TIMES
.ENDC
INC (DAT) ;CHANGE "NXTFIN" TO "FINAL" FLAG
BIC #NXTFIN+FIRST,(DAT)
BIT #MOVING+RUN,(DAT) ;CHECK IF SERVOING COMPLETED
BEQ SRVDNE ;KILL SERVO IF DONE
br svloop ;GO do the other servos that are waiting
;kill the single servo that finished
srvdne: bis #srvkil,(dat) ;set servo killed bit
DECB @INUSE(DAT) ;CHECK IF OTHER ATTACHED SERVOS STILL ACTIVE
bne svloop ;more servos left, go do next one
srvrtn: rts pc ;return to SERVO
.IFNZ DIAGY
SAVDAT: MOV DBUF,EC ;SETUP DIAGNOSTIC DATA BUFFER
MOV PTIME(DAT),(EC)+ ;CURRENT TIME
TST ACTUAL ;CHECK IF ERROR READINGS TO BE SAVED
BNE SAVACT
LDF POSERR(DAT),AC0 ;SEND BACK POSITION AND VELOCITY ERROR
LDF VELER,AC1
BR SAVRDG
SAVACT: LDF TH(DAT),AC0 ;SEND BACK ACTUAL JOINT ANGLE AND VELOCITY
LDF THD(DAT),AC1
SAVRDG: STF AC0,(EC)+
STF AC1,(EC)+
BIT #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFAACE *KM
BEQ 1$
MOVB DACVAL(DAT),AC ;SAVE OUTPUT TORQUE *KF CHANGE FOR YELLOW I.
BR 2$
1$: MOV DACCHN(DAT),AC
BIC #170000,AC
BIT #4000,AC ;IS IT NEG.?
BEQ 2$
BIS #170000,AC ;YES EXTEND SIGN
2$: MOV AC,(EC)+
MOV EC,DBUF ;SAVE BUFFER POINTER FOR NEXT TIME
BR SDRET
.ENDC
;local storage area
DATA
sdtime==16. ;minimum time between scheduling servo for
; running, in milliseconds.
dvarrp: .word 0 ;pointer to the device status array
devpnt: .word 0 ;pointer to the device block
cursrv: .word 0 ;pointer to current servo in the device block
; being serviced
schtim: .word 0 ;scheduled time that servo will be run next
CODE
;END OF "SERVO"
; STPMVE, STPRUN, CATERR - Stopping joint motion routines
;"STPMVE" - SET BRAKES, CLEAR RUN FLAG, SET TIMEOUT COUNT, AND WAIT FOR ZERO VELO.
STPMVE: BIC #RUN,(DAT) ;INDICATE SERVO NOT RUNNING ANYMORE
MOV #STPTME,COUNT(DAT) ;TIME ALLOWED FOR JOINT TO STOP MOVING
RTS PC ;EXIT
;END OF "STPMVE"
;"STPRUN" - STOP SERVO NOW (SETS BRAKES AND CLEARS FLAGS)
STPRUN: BIC #RUN+MOVING,(DAT) ;INDICATE KILL SERVO NOW
JSR PC,MOTSTP ;SET BRAKES AND STOP MOTOR DRIVE
RTS PC
;"CATERR" - CATASTROPHIC ERROR, SIGNAL ALL SERVOS ATTACHED TO OUR DEVICE
CATERR: BIS #100000,@INUSE(DAT) ;SIGNAL FATAL ERROR OCCURRED
BIC #RUN,(DAT) ;INDICATE SERVO NOT RUNNING ANYMORE
MOV #STPTME,COUNT(DAT) ;TIME ALLOWED FOR JOINT TO STOP MOVING
RTS PC ;EXIT
;END OF "CATERR"
; ANGLES - converts A/D readings to joint angle and velocity
;LEAVES THE NEW JOINT ANGLE IN "TH" AND ACO, AND THE NEW ANGULAR VELOCITY
;IN "THD" AND AC1. IF THIS SERVO HAS NO TACHOMETER ( INDICATED BY TACCHN
;BEING ZERO ) OR IF THE TACH VALUE IS NEARLY OUT OF THE A/D READING RANGE,
;THE CURRENT AND PREVIOUS JOINT ANGLES ARE DIFFERENCED TO DETERMINE THE
;ANGULAR VELOCITY.
;REGISTERS USED:
;
; AC,BC,CC,DC GARBAGED
; "TH" LEFT IN AC0, "THD" LEFT IN AC1
;EXECUTION TIME: 130 MICRO SECONDS WITH TACH
; 110 MICRO SECONDS WITHOUT TACH
;COMPUTE JOINT ANGLE
ANGLES: bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 3$
MOVB POTCHN(DAT),ADCCHN ;START CONVERTING POT READING
LDF TH(DAT),AC1 ;GET THE PREVIOUS JOINT ANGLE
TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,AC ;GET POT READING
MOVB TACCHN(DAT),ADCCHN ;START CONVERTING TACH READING
MOV AC,POT(DAT)
ADD #4000,AC ;SHIFT POT READING TO 0 → 7777
BR 4$
3$: MOV #42,DR11S ;SET YELLOW INTERFACE MODE
MOV POTCHN(DAT),DR11O
LDF TH(DAT),AC1 ;GET THE PREVIOUS JOINT ANGLE
TSTB DR11S ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV DR11I,AC ;GET POT READING
MOVB TACCHN(DAT),DR11O ;START CONVERTING TACH READING
MOV AC,POT(DAT)
4$:
;ADD CORRECTION FOR NON-LINEAR POT ELEMENT
.IFZ ISLIN
MOV AC,DC ;SAVE NORMALIZED POT READING
CLR BC
ASHC #-10,AC ;GET 4 MSB TO USE AS INDEX INTO NON-LINEAR TABLE
ROR BC ;LEAVE 8 LSB AS POSITIVE TWOS COMPLEMENT NUMBER
ADD DAT,AC ;COMPUTE A POINTER INTO THE NON-LINEAR CORR. TABLE
MOVB NONLIN(AC),CC ;GET LOWER LIMIT OF CALIBRATION TABLE
MOVB NONLIN+1(AC),AC ;GET UPPER LIMIT TO INTERPOLATE
SUB CC,AC ;GET DIFFERENCE
MUL BC,AC ;INTERPOLATE
ASHC #1,AC ;GET 16 MSB
ADD CC,AC ;NOW HAVE PROPER CALIBRATION
ADD DC,AC ;ADD TO NORMALIZED POT READING
.ENDC
;CONVERT TO JOINT ANGLE
LDCIF AC,AC0 ;CONVERT TO FLOATING POINT
MULF SCALE(DAT),AC0 ;CONVERT FROM A/D UNITS TO JOINT ANGLE
ADDF OFFSET(DAT),AC0 ;ADD POT OFFSET
STF AC0,TH(DAT) ;SAVE THE JOINT ANGLE WITH THE SERVO DATA
;COMPUTE THE ANGULAR VELOCITY OF THE JOINT
.IFZ TACCAL
TST TACCHN(DAT) ;CHECK IF THERE IS A A/D TACH CHANNEL
BGT 2$ ;JUMP IF WE DO HAVE A TACHOMETER
1$: SUBF AC0,AC1 ;NO TACH, DIFFERENCE POSITION READINGS
NEGF AC1
DIVF ELTIME(DAT),AC1 ;DIVID BY ELAPSED TIME BETWEEN READINGS
STF AC1,THD(DAT) ;SAVE VELOCITY( DEG/TICK )
RTS PC ;RETURN
2$: bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 6$
TSTB ADCSR ;WAIT TILL ADC CONVERSION DONE
BPL .-4
MOV ADCVAL,AC ;GET TACH READING
BR 7$
6$: TSTB DR11S
BPL .-4
MOV DR11I,AC
SUB #3777,AC ;OFFSET SO 0=0
7$: SUB TACHZ0(DAT),AC ;SUBTRACT ZERO VELOCITY TACH READINGS
MOV AC,BC
BPL 8$
NEG BC
8$: bit #BLUARM+BLUHND,MECHSM(DAT) ;SKIP RANGE CHECK IF YELLOW
BEQ 11$
CMP #3400,BC ;CHECK IF READING OUT OF RANGE
BLE 1$ ;DIFFERENCE POSITION IF SO
11$: BIC #7,BC ;VEL = 0 IF IN NOISE BAND
BNE .+4
CLR AC
LDCIF AC,AC1 ;CONVERT TO FLOATING POINT
MULF VSCALE(DAT),AC1 ;CONVERT FROM A/D UNITS TO DEGREES/TICK
STF AC1,THD(DAT) ;SAVE VELOCITY
RTS PC
.IFF
2$: bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 6$
TSTB ADCSR ;WAIT TILL ADC CONVERSION DONE
BPL .-4
MOV ADCVAL,AC ;GET TACH READING
BR 7$
6$: TSTB DR11S
BPL .-4
MOV DR11I,AC ;GET TACH READING
7$: LDCIF AC,AC1 ;ACCUMULATE TACH READINGS
TST AC ;GET ABS VALUE OF READING
BGE .+4
NEG AC
ADDF TTACH,AC1
CMP #3400,AC ;CHECK IF READING OUT OF RANGE
BGT .+6
INC TACOFR ;KEEP TRACK OF NUMBER OF READINGS OUT OF RANGE
STF AC1,TTACH
INC TACRDG ;KEEP TRACK OF NUMBER OF READINGS TAKEN
CLRF AC1 ;SET ZERO VELOCITY
STF AC1,THD(DAT) ;SAVE VELOCITY
RTS PC
DATA
TTACH: .BLKW 2 ;ACCUMULATED TACH READINGS FOR TACCAL MODE
TACRDG: 0 ;NUMBER OF TACH READINGS TAKEN
TACOFR: 0 ;NUMBER OF TACH READINGS OUT OF RANGE
CODE
.ENDC
;END OF "ANGLES"
; FEEDBK - servo feedback loop to drive the joint motors
;COMPUTES THE OUTPUT MOTOR DRIVE BY DETERMINING THE POSITION AND VELOCITY ERROR
;AND ADDING IN THE EFFECTS OF GRAVITY LOADING, FRICTION, AND INERTIAL ACCELERATION.
;THE IMPLEMENTED EQUATION IS AS FOLLOWS:
;
; MOTOR TORQUE = CII*THDDP + CI - KE*(TH-THP) - KV*CII*(THD-THDP)
; - (KI/CII)*INTEGRAL(TH-THP) + or - V0
; WHERE
; CII = JOINT INERTIA
; CI = JOINT GRAVITY LOADING
; TH = ACTUAL JOINT ANGLE
; THP = PREDICTED JOINT ANGLE
; THD = ACTUAL JOINT ANGULAR VELOCITY
; THDP = PREDICTED JOINT ANGULAR VELOCITY
; THDDP= PREDICTED JOINT ANGULAR ACCELERATION
; V0 = FRICTION COMPENSATION WITH THE SAME SIGN AS VELOCITY
; KE,KI,KV = FEEDBACK COEFFICIENTS
;
;THE INTEGRAL OF POSITION ERROR IS DEVELOPED BY SUMMING THE POSITION ERROR EACH
;TIME THE SERVO IS SERVICED.
;EXECUTION TIME: 240 MICRO SECONDS
;REGISTERS USED:
; AC0 MUST BE LOADED WITH "TH" AND AC1 "THD" PRIOR TO CALLING FEEDBK
; AND THE INERTAL TORQUE MUST BE LOADED INTO AC3 IF ON TRAJECTORY
FEEDBK: SUBF THP(DAT),AC0 ;SUBT THE PREDICTED JOINT ANGLE FROM THE ACTUAL
BIT #FINAL,(DAT) ;CHECK IF MOTION COMPLETED
BEQ CMPFBK ;BRANCH IF STILL ON TRAJECTORY
;AT END OF TRAJECTORY, NULL ERRORS AND EXIT IF WITHIN TOLERANCE
BIC #FSERVO,(DAT) ;DON'T FORCE SERVO DURING NULLING TIME
CLRF AC3 ;IGNOR JOINT INERTIAL ACCELERATION
CLRF THDP(DAT) ;PREDICTED VELOCITY MUST BE ZERO
BIT #NNUL,MODBTS(DAT);CHECK IF WE NULL ERRORS AT END OF TRAJ.
BEQ 1$
JSR PC,STPMVE ;NO NULLING, GO STOP MOVING
JSR PC,MOTSTP
BR 2$
1$: STF AC0,AC2 ;NULLING, CHECK IF WITHIN FINAL TOLERANCE
ABSF AC2 ;GET ABS( POSITION ERROR )
CMPF ERRTOL(DAT),AC2 ;COMPARE TO PERMITTED TOLERANCE
CFCC
BLT cmpfbk ;IF ERROR TOO LARGE, USE FEEDBACK TO REDUCE IT
2$: STF AC1,AC2 ;GET ABS( VELOCITY )
ABSF AC2
CMPF ERRTOL(DAT),AC2 ;COMPARE TO PERMITTED VELOCITY TOLERANCE
CFCC
BLT cmpfbk ;IF TOO LARGE, CONTINUE SERVOING TO SET POINT
JSR PC,STPRUN ;NOW WITHIN TOLERANCE, KILL SERVO
RTS PC
;COMPUTE FEEDBACK ERROR TERMS AND CORRECT MOTOR DRIVE
CMPFBK: STF AC0,POSERR(DAT) ;SAVE THE POSITION ERROR
BIT #FSERVO,(DAT) ;FORCE SERVOING?
BEQ 1$
LDF FLEVEL(DAT),AC0 ;YES, GET COMPLIANCE LEVEL
; STF AC0,ERRTQE(DAT)
BR 2$ ;JUST ADD GRAVITY LOADING AND FRICTION COMP.
1$: STF AC0,AC2
.IFZ TACCAL
SUBF THDP(DAT),AC1 ;SUBT THE PREDICTED ANGULAR VELOCITY FROM THE ACTUAL
.ENDC
.IFNZ DIAGY
STF AC1,VELER ;SAVE THE VELOCITY ERROR
.ENDC
ADDF ERRINT(DAT),AC2 ;SUM THE INTEGRAL POSITION ERROR
STF AC2,ERRINT(DAT)
MULF KI(DAT),AC2 ;MULTIPLY INTEGRAL BY FEEDBACK COEF., KI
DIVF CII(DAT),AC2 ;FOR STABILITY, DIVID KI BY JOINT INERTIA
MULF KE(DAT),AC0 ;MULT THE POSITION ERROR BY FEEDBACK COEF.,KE
ADDF AC2,AC0 ;POSITION + INTEGRAL ERROR FEEDBACK
MULF KV(DAT),AC1 ;MULTIPLY VELOCITY ERROR BY FEEDBACK COEF., KV
MULF CII(DAT),AC1 ;FOR STABILITY, MULT KV BY JOINT INERTIA
ADDF AC1,AC0 ;TOTAL TORQUE FEEDBACK
MULF TOTQE(DAT),AC0 ;CONVERT TO UNITS OF TORQUE
; STF AC0,ERRTQE(DAT) ;SAVE THE JOINT ERROR TORQUE
ADDF AC3,AC0 ;ADD THE INERTIAL TORQUE
2$: ADDF CI(DAT),AC0 ;ADD GRAVITY LOADING
;ADD IN THE EFFECTS OF THE PREDICTED FRICTIONAL RESISTANCE
BIT #FINAL,(DAT) ;CHECK IF JUST NULLING ERRORS
BNE 3$ ;DON'T LOOK AT VELOCITY IF SO
MOV THD(DAT),AC ;GET THE SIGN AND THE MSB OF ACTUAL JOINT VELOCITY
BNE 4$
BIT #FSERVO,(DAT) ;IF FORCE SERVOING POSERR INVALID
BNE 6$
3$: MOV POSERR(DAT),AC ;USE THE PREDICTED TH - ACTUAL TH IF VELOCITY ZERO
BEQ 6$ ;DONT ADD ANY FRICTION IF BOTH VEL AND POS ERROR ZERO
NEG AC
4$: BLT 5$ ;FRICTION RESISTS MOVEMENT, CHECK DIRECTION OF VEL.
ADDF V0(DAT),AC0 ;POS. VELOCITY, COMPENSATE FOR FRICT. BY ADDING V0
BR 6$
5$: SUBF V0(DAT),AC0 ;NEG. VELOCITY, " " " " SUBT. V0
6$: STF AC0,ERRTQE(DAT)
JSR PC,MOTDRV ;DRIVE MOTOR
RTS PC ;RETURN
;END OF "FEEDBK"
; EVAL - fetches polynomial coef. and evaluates for next servicing time
;THE JOINT ACCELERATION IS COMPUTED BY DETERMINING THE CONSTANT ACCELERATION
;THAT WILL PRODUCE THE SAME CHANGE IN ANGLE BETWEEN NOW AND THE NEXT SERVICING
;PERIOD AS THE 5TH ORDER POLYNOMIAL. THE IMPLEMENTED EQUATION IS AS FOLLOWS:
;
; ACCELERATION = ( V2 - V1 )/DT
; V2 = ( P2 - P1 )/DT
; P2 = P2' + DELTHD*DT + WOBBLE
;
; WHERE P1 = PREDICTED JOINT ANGLE FOR CURRENT SERVICING PERIOD
; V1 = " " VELOCITY " " " "
; P2 = " " ANGLE FOR THE NEXT SCHEDULED PERIOD
; P2' = POLYNOMIAL JOINT " " " " " "
; DELTHD= VELOCITY DEVIATION OF RUN TIME PATH FROM POLY. VALUE
; DT = INTERVAL OF TIME UNTIL WE ARE SCHEDULED TO BE SERVICED
; WOBBLE= SINUSOIDAL WOBBLE IN JTS 4,5,6
;
;THE PREDICTED JOINT POSITION AND VELOCITY FOR THE NEXT SERVICING PERIOD ARE
;STORED IN "THN(DAT)" AND "THDN(DAT)" AND THE INERTIAL TORQUE IS LEFT IN AC3
;
;ALSO, THE CURRENT JOINT INERTIA AND GRAVITY LOADING IS DETERMINED BY
;INTERPOLATION. THE COMPUTED VALUES ARE SAVED IN CII AND CI.
;EXECUTION TIME: 320 MICRO SEC. WHILE CHANGING SEGMENTS
; 310 MICRO SEC. FOR IN MIDDLE OF SEGMENT
; +100 MICRO SEC. IF JOINT OUT OF RANGE
;REGISTERS USED:
; EXPECTS VALUE OF ELTIME IN AC2
; LEAVES INERTIAL TORQUE IN AC3
EVAL: MOV DATPT(DAT),CC ;GET POINTER TO START OF POLY. DATA LIST
BNE 1$
;NOT ON POLYNOMIAL, JUST COMPUTE CHANGE DUE TO CONSTANT VELOCITY FACTOR
CLRF AC0 ;ZERO POLYNOMIAL POSITION
LDF CII(DAT),AC3 ;USE CONSTANT JOINT INERTIA
JMP NUDGE ;GO ADD IN CONSTANT VELOCITY FACTOR
;ON POLYNOMIAL, CHECK IF END OF SEGMENT OR START OF FIRST SEGMENT
1$: MOV TTIME(DAT),AC ;GET PRESENT TOTAL SEGMENT TIME
BIT #FIRST,(DAT) ;ARE WE JUST STARTING A MOTION?
BNE FSTSEG ;GO SET FIRST SEGMENT TIME
CMP PTIME(DAT),AC ;CHECK IF PAST END OF SEGMENT
BLT EVALPY ;BRANCH IF STILL IN SAME SEGMENT
;SWITCHING TO A NEW SEGMENT SAVE DYNAMIC COEF. AND CHECK IF FINAL SEGMENT
CHKFIN: MOV FCI(DAT),BC ;GET POINTER TO GRAV. LDG. AND INERTIA
MOV (BC)+,NCI(DAT) ;FINAL GRAV. LDG. OF PAST SEG= INITIAL GRAV. LDG
MOV (BC)+,NCI+2(DAT); OF NEXT SEGMENT
MOV (BC)+,NCII(DAT) ;SAME FOR JOINT INERTIA
MOV (BC),NCII+2(DAT)
MOV RNCODE(CC),BC ;CHECK IF POSSIBLE EVENT TO SIGNAL
BEQ 2$ ;ZERO IF NONE OR ALREADY SIGNALED
MOV AC,-(SP) ;Need to save registers - Kernel will clobber them
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOV DAT,-(SP)
EVSIG BC
MOV (SP)+,DAT ;Restore registers
MOV (SP)+,EC
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,AC
CLR RNCODE(CC) ;ZERO TO INDICATE SIGNALED
2$: MOV (CC),BC ;SAVE INCREMENT TO NEXT SEGMENT
ADD BC,CC ;GET THE POINTER TO THE START OF THE NEXT SEGMENT
TST (CC) ;CHECK IF FINAL SEGMENT
BNE 1$ ;BRANCH IF NOT
;FINISHED MOTION, COMPUTE FINAL POSITION AND DYNAMIC COEF.
MOV POLY(DAT),CC ;GET POINTER TO POLYNOMIAL LIST
LDF (CC),AC0 ;COMPUTE FINAL SET POINT FROM POLY, GET A5
ADDF -(CC),AC0 ; + A4
ADDF -(CC),AC0 ; + A3
ADDF -(CC),AC0 ; + A2
ADDF -(CC),AC0 ; + A1
ADDF -(CC),AC0 ; + A0
MOV FCI(DAT),CC ;GET PTR TO GRAV. LDG. AND INERTIA
LDF 4(CC),AC3 ;SET FINAL VALUE OF JOINT INERTIA
MOV (CC)+,CI(DAT) ;SET FINAL VALUE OF GRAVITY LOADING
MOV (CC),CI+2(DAT)
STF AC3,CII(DAT)
CLR DATPT(DAT) ;INDICATE NO MORE POLYNOMIALS
BIS #NXTFIN,(DAT) ;INDICATE NEXT SERVICING PERIOD IS LAST
MOV #NULTME,COUNT(DAT) ;ALLOW NULTME SECONDS TO NULL ERRORS
JMP NUDGE ;GO ADD IN CONSTANT VELOCITY FACTOR
;NOT THE FINAL SEGMENT, UPDATE POINTERS, DYNAMIC COEF, AND SEG. TIME
1$: MOV CC,DATPT(DAT) ;SAVE NEW SEGMENT POINTER
ADD BC,FCI(DAT) ;UPDATE OTHER DATA POINTERS
ADD BC,POLY(DAT)
SUB AC,PTIME(DAT) ;GET TIME INTO NEXT SEGMENT
FSTSEG: MOV SEGTME(CC),AC ;SET THE TOTAL TIME OF THE NEXT SEG
LDCIF AC,AC0 ;FLOAT THE TOTAL SEGMENT TIME
CMP PTIME(DAT),AC ;CHECK IF PAST THIS NEW SEGMENT
BGE CHKFIN
MOV AC,TTIME(DAT) ;SAVE THE NEW TOTAL SEGMENT TIME
STF AC0,FTTIME(DAT) ;SAVE THE F.P. TOTAL TIME
MOV FCI(DAT),AC ;COMPUTE CHANGE IN GRAVITY LOADING
LDF (AC)+,AC3
SUBF NCI(DAT),AC3
STF AC3,DCI(DAT)
LDF (AC),AC3 ;COMPUTE CHANGE IN INERTA
SUBF NCII(DAT),AC3
STF AC3,DCII(DAT)
;EVALUATE THE POSITION POLYNOMIAL FOR THE NEXT SERVICING PERIOD.
EVALPY: LDCIF PTIME(DAT),AC1 ;CONVERT THE PRESENT TIME TO FLOATING POINT
MOV POLY(DAT),CC ;GET ABSOLUTE ADDRESS OF POSITION POLYNOMIAL
DIVF FTTIME(DAT),AC1 ;GET THE FRACTIONAL TIME INTO THIS SEGMENT
LDF (CC),AC0 ;EVALUATE THE POSITION POLYNOMIAL, GET A5
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A4
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A3
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A2
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A1
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A0, NOW HAVE POLYNOMIAL JOINT ANGLE
;INTERPOLATE GRAVITY LOADING AND JOINT INERTIA
LDF DCI(DAT),AC3 ;GET CHANGE IN GRAVITY LOADING
MULF AC1,AC3 ; x TIME INTO SEGMENT
ADDF NCI(DAT),AC3 ; + INITIAL G.L. FOR SEGMENT
STF AC3,CI(DAT) ;SAVE NEW GRAVITY LOADING
LDF DCII(DAT),AC3 ;REPEAT FOR JOINT INERTIA
MULF AC1,AC3
ADDF NCII(DAT),AC3
STF AC3,CII(DAT) ;SAVE NEW JOINT INERTIA
;ADD IN A CONSTANT VELOCITY FACTOR TO MODIFY THE TRAJECTORY WHILE RUNNING
NUDGE: LDF DELTHD(DAT),AC1 ;LOAD THE VELOCITY FACTOR
ldf eltime(dat),ac2 ;load elapsed time
MULF AC2,AC1 ;VELOCITY * ELAPED TIME = CHANGE IN POSITION
ADDF DELTH(DAT),AC1 ;DETERMINE THE TOTAL POSITION DEVIATION
ADDF AC1,AC0 ;MODIFY NEXT POSITION
STF AC1,DELTH(DAT) ;SAVE THE TOTAL POSITION DEVIATION
;ADD IN WOBBLE IN ANGLE IN LAST 3 JOINTS
BIT #WOBBLE,MODBTS(DAT) ;WOBBLING?
BEQ NOWOB ;NO
MOV WOBPTR(DAT),AC ;INDEX TO SINE TABLE
BLT NOWOB ;ONLY WOBBLE JTS 4,5,6
LDF WOBTAB(AC),AC1 ;COMPUTE WOBBLE SINUSOID
MULF @WOBMAG(DAT),AC1
CMP (AC)+,(AC)+ ;INDEX AND SAVE PTR
BIC #177700,AC
MOV AC,WOBPTR(DAT)
ADDF AC1,AC0 ;ADD WOBBLE TO NEW POSITION
;CHECK IF JOINT GOING BEYOND STOP LIMIT
NOWOB: BIC #PASLIM,(DAT) ;ASSUME JOINT IN RANGE
CMPF USTOP(DAT),AC0 ;COMPARE TO UPPER STOP LIMIT
CFCC
BGE 2$ ;BRANCH IF WITHIN LIMIT
BIS #PASLIM,(DAT) ;INDICATE JOINT OUT OF RANGE
BIT #FIRST+STROUT,(DAT) ;CHECK IF JOINT STARTED OUTSIDE OF RANGE
BEQ 1$ ;BRANCH IF IT WASN'T
BIS #STROUT,(DAT) ;INDICATE JOINT STARTED OUT OF RANGE
CMPF THP(DAT),AC0 ;COMPARE NEW SET POINT TO OLD
CFCC
BGE SVENTH ;ONLY PERMIT THE JOINT TO MOVE IN RANGE
LDF THP(DAT),AC0
BR SVENTH
1$: SUBF USTOP(DAT),AC0 ;COMPUTE DISTANCE BEYOND STOP LIMIT
STF AC0,AC1
ADDF STPBND(DAT),AC0 ;(TH-STOP)+(WIDTH OF STOP BAND)
DIVF AC0,AC1 ;PERCENTAGE INTO THE STOP BAND
MULF STPBND(DAT),AC1 ;NEW ANGLE BEYOND STOP LIMIT
LDF USTOP(DAT),AC0 ;NEW ANGLE IS STOP LIMIT+DISTANCE INTO STOP BAND
ADDF AC1,AC0
BR SVENTH
2$: CMPF LSTOP(DAT),AC0 ;COMPARE TO LOWER STOP LIMIT
CFCC
BLE JTINRG ;BRANCH IF WITHIN LIMIT
BIS #PASLIM,(DAT) ;INDICATE JOINT OUT OF RANGE
BIT #FIRST+STROUT,(DAT) ;CHECK IF JOINT STARTED OUTSIDE OF RANGE
BEQ 3$ ;BRANCH IF IT WASN'T
BIS #STROUT,(DAT) ;INDICATE JOINT STARTED OUT OF RANGE
CMPF THP(DAT),AC0 ;COMPARE NEW SET POINT TO OLD
CFCC
BLE SVENTH ;ONLY PERMIT THE JOINT TO MOVE IN RANGE
LDF THP(DAT),AC0
BR SVENTH
3$: SUBF LSTOP(DAT),AC0 ;SAME PROCESS AS BEYOND UPPER LIMIT
STF AC0,AC1
SUBF STPBND(DAT),AC0
DIVF AC0,AC1
MULF STPBND(DAT),AC1
LDF LSTOP(DAT),AC0
SUBF AC1,AC0
JTINRG: BIC #STROUT,(DAT) ;INDICATE JOINT NOT STARTED OUT OF RANGE
;COMPUTE JOINT INERTIAL ACCELERATION AND TORQUE
SVENTH: STF AC0,THN(DAT) ;SAVE THE PREDICTED JOINT ANGLE FOR NEXT TIME
SUBF THP(DAT),AC0 ;SUBT CURRENT JOINT ANGLE FROM NEXT PREDICTED ANGLE
DIVF AC2,AC0 ;DIVID BY ELAPED TIME
STF AC0,THDN(DAT) ;SAVE THE PREDICTED VELOCITY FOR NEXT SERVICING TIME
SUBF THDP(DAT),AC0 ;SUBT CURRENT PREDICTED VELOCITY
DIVF AC2,AC0 ;DIVID BY TIME AGAIN
MULF TOTQE(DAT),AC3 ;CONVERT DEGREES TO RADIANS IF NECESSARY
MULF AC0,AC3 ;MULT BY JOINT INERTIA AND LEAVE VALUE OF
; NEW INERTIAL TORQUE IN AC3
RTS PC ;RETURN
;END OF "EVAL"
; WIPER - switches A/D channels if A/D out of range & servo has multiple wipers
;OR'S A 1 INTO "OUTRGE" IF A WIPER WAS SWITCHED. ASSUMES "WIPERS" POINTS TO
;A LIST OF DATA ARRANGED AS FOLLOWS:
;
; .BLKW 0,0,0,0,0 ;BOTTOM END OF LIST
; :
; A/D CHAN # ;ONE WORD INCREASING READING
; F.P. OFFSET ;TWO WORDS ↓
; F.P. SCALE ;TWO WORDS
; WIPERS → A/D CHAN # ;ONE WORD
; F.P. OFFSET ;TWO WORDS
; F.P. SCALE ;TWO WORDS
; :
; .BLKW 0 ;TOP END OF LIST
;REGISTERS USED:
;
; AC GARBAGED, DAT REFERENCED
WIPER: MOV WIPERS(DAT),AC ;GET THE POINTER TO THE WIPER DATA
BNE .+4
RTS PC ;RETURN IF SERVO HAS ONLY ONE WIPER
bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT ARM
BNE 11$
CMP POT(DAT),#100. ;CHECK IF A/D READING OUT OF RANGE
BGT 2$ ;BR IF NOT TOO LOW
BR 12$ ;ELSE CHANGE WIPER
11$: CMP POT(DAT),#-2400 ;CHECK IF A/D READING OUT OF RANGE
BGT 2$ ;JUMP IF NOT TOO LOW
12$: SUB #12,AC ;TOO LOW- POINT TO PREVIOUS WIPER DATA
1$: TST (AC) ;CHECK IF AT END OF WIPER LIST
BNE .+4
RTS PC ;RETURN IF WE CAN'T SWITCH TO A NEW WIPER
MOV AC,WIPERS(DAT) ;ELSE SAVE THE POINTER TO THE NEW WIPER DATA
MOV (AC)+,POTCHN(DAT) ;SAVE THE NEW A/D CHANNEL NUMBER
MOV (AC)+,OFFSET(DAT) ;SAVE THE NEW F.P. OFFSET VALUE
MOV (AC)+,OFFSET+2(DAT)
MOV (AC)+,SCALE(DAT) ;SAVE NEW F.P. SCALE FACTOR
MOV (AC),SCALE+2(DAT)
BIS #1,OUTRGE ;INDICATE WIPER OUT OF RANGE
RTS PC ;RETURN
2$: bit #BLUARM+BLUHND,MECHSM(DAT)
BNE 21$
CMP POT(DAT),#3400.
BGE 22$ ;BR IF TOO HIGH
RTS PC ;ELSE IN RANGE, RETURN
21$: CMP POT(DAT),#2400 ;CHECK IF A/D READING TOO HIGH
BGE .+4
RTS PC ;RETURN IF WITHIN RANGE
22$: ADD #12,AC ;ELSE, POINT TO THE NEXT WIPER DATA
BR 1$ ;GO SWITCH WIPERS
DATA
OUTRGE: 0 ;"WIPER" OR'S A 1 INTO THIS WORD IF POT WIPER OUT OF RANGE,
; NEVER RESET BY SERVO CODE.
CODE
;END OF "WIPER"
; MOTDRV, MOTSTP, BRKREL - motor drive routines
;"MOTDRV" SETS THE MOTOR DRIVE FOR CURRENT SERVO. THE OUTPUT TORQUE MUST BE
;LOADED INTO AC0 BEFORE THE CALL TO "MOTDRV". IF THE DRIVE REQUESTED IS TOO
;LARGE, "EXJTFC" IS SET IN THE STATUS WORD AND ALL SERVOS ATTACHED TO THE SAME
;DEVICE AS THIS JOINT ARE STOPPED. THE FINAL DESIRED MOTOR TORQUE IS THE
;SUM OF THE VALUE PLACED IN AC0 BEFORE THIS ROUTINE WAS CALLED AND A DITHER
;TERM.
;EXECUTION TIME: 40 MICRO SEC. IF NO CATASTROPHIC ERROR
MOTDRV: MULF MSCALE(DAT),AC0 ;CONVERT THE MOTOR TORQUE TO A/D UNITS
NEG DITHER(DAT) ;COMPLEMENT THE JOINT DITHER TERM
STCFI AC0,CC ;CONVERT THE MOTOR DRIVE TO INTEGER
ADD DITHER(DAT),CC ;ADD DITHER TO JOINT TORQUE
MOV CC,DC ;GET ABSOLUTE VALUE
BGE .+4
NEG DC
CMP MAXDRV(DAT),DC ;CHECK IF DRIVE TOO LARGE
BGE 1$ ;BRANCH IF WITHIN RANGE
BIS #EXJTFC,(DAT) ;INDICATE EXCESSIVE JOINT FORCE
JSR PC,CATERR ;CATASTROPHIC ERROR, GO STOP ALL RELATED SERVOS
BR MOTSTP ;STOP MOTION OF THIS JOINT
1$: bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFAACE *KM
BEQ 2$ ;BR IF YELLOW INTERFACE
MOVB CC,DACVAL(DAT) ;SET DAC OUTPUT VALUE
MOV DACCHN(DAT),@DACADD(DAT) ;SEND TO DAC
RTS PC ;RETURN
2$: BIC #170000,CC ;MASK TO 12 BITS FOR YELLOW INTERFACE
BIC #7777,DACCHN(DAT) ;CLEAR PREVIOUS VALUE
BIS CC,DACCHN(DAT) ;*K
MOV #40,DR11S ;CODE TO WRITE TO A/D AND KEEP PANIC INTERUPT ENB.
MOV DACCHN(DAT),DR11O ;SEND TO DAC
RTS PC
;"BRKREL" RELEASES THE BRAKE FOR THE MOTOR BY CLEARING THE BRAKE BIT IN THE DAC REG
;EXECUTION TIME:
BRKREL: bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFAACE *KM
BEQ 1$
MOV DACADD(DAT),AC ;GET THE DAC ADDRESS
BISB DACINF(DAT),2(AC) ;CLEAR THE BRAKE BIT
RTS PC ;RETURN
1$: MOV #20.,-(SP) ;THIS IS DUMB
2$: MOV #41,DR11S ;SET BRAKE MODE AND INT ENB B
BIS dacinf(dat),DR11O ;release BRAKE AND ENABLE INT B
DEC (SP)
BGT 2$
TST (SP)+ ;CLEAR COUNT OFF STACK
; MOV DR11O,@BRAKEP ;*D
; ADD #2,BRAKEP
RTS PC
;"MOTSTP" RESETS THE MOTOR DRIVE AND SETS THE BRAKE
;EXECUTION TIME:
MOTSTP: bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFAACE *KM
BEQ 1$ ;BR IF YELLOW INTERFACE
MOV DACADD(DAT),AC ;GET THE DAC ADDRESS
CLRB DACVAL(DAT) ;ZERO DAC OUTPUT
MOV DACCHN(DAT),(AC)+ ;SET A ZERO MOTOR DRIVE
BICB DACINF(DAT),(AC) ;SET THE BRAKE
RTS PC
1$: MOV #40,DR11S ;CODE TO WRITE TO A/D AND KEEP PANIC INTERUPT ENB.
BIC #7777,DACCHN(DAT) ;ZERO DRIVE VALUE
MOV DACCHN(DAT),DR11O ;SEND TO INTERFACE
MOV #41,DR11S ;SET BRAKE MODE AND INT ENB B
BICB DACINF(DAT),DR11O ;SET BRAKE
RTS PC
;END OF MOTOR DRIVE FUNCTIONS
;DATA
;BRAKEP: .WORD 0 ;*D
CODE
;INTARM - initializes servo code for future arm motions
;THIS ROUTINE SHOULD BE CALLED AT LEAST ONCE BEFORE ANY ARM MOTIONS ARE
;INITIATED. THE FOLLOWING OPERATIONS ARE PERFORMED:
;
; 1) THE PANIC BUTTON INTERRUPT VECTOR IS SETUP TO SET THE VARIABLE
; "BPANIC" NON-ZERO WHENEVER THE PANIC BUTTON IS HIT WHILE ENABLED.
; 2) THE REFERENCE POWER SUPPLY VOLTAGE IS READ AND ITS VALUE IS
; COMPARED TO ITS VALUE AT THE TIME THE ARM WAS CALIBRATED. IF A
; SIGNIFICANT DISCREPENCY EXISTS, A ERROR CODE IS RETURNED.
; 3) A ZERO VELOCITY TACHOMETER READING IS TAKEN FOR ALL OPERATIONAL
; JOINTS WITH TACHS. THE ZERO READING IS STORED IN THE "TACHZ0"
; WORD OF THE SERVO DATA BLOCKS. IF A READING DEVIATES BY MORE
; THAN 10 ADC COUNTS FROM THE INITIAL "TACHZ0" VALUE, AN ERROR
; IS SIGNALED.
; 4) THE CURRENT JOINT ANGLE FOR ALL OPERATIONAL JOINTS IS READ AND
; SAVED IN THE SERVO DATA BLOCKS. AT THE SAME TIME, THE GRAVITY
; LOADING AND INERTIA CONSTANT FOR EACH JOINT IS COMPUTED.
; 5) THE SERVO DATA BLOCKS FOR ALL OPERATIONAL JOINTS ARE INITIALIZED.
; 6) FORCE EVENT LIST IS INITIALIZED AND ALL FORCE SENSING IS
; TERMINATED.
; 7) THE LEVEL 6 SERVO JOB IS SCHEDULED
; 8) IT DOES NOT INITIALIZE THE PUMA ARMS -- CALIB SHOULD BE
; CALLED TO PERFORM THAT FUNCTION.
;
;THIS ROUTINE REQUIRES AS ITS ONLY ARGUMENT THE ADDRESS OF A DEVICE BLOCK WHICH
;IS AT LEAST 2+N WORDS LONG, WHERE N IS THE NUMBER OF JOINTS CURRENTLY IN
;EXISTANCE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;DEVICE STORAGE BLOCK
; JSR PC,INTARM
; TST R0 ;CHECK FOR ERROR CONDITIONS
;
;AT THE END OF EXECUTION, REGISTER R0 RETURNS THE ERROR CODE GENERATED BY THIS
;ROUTINE. THE INTERPRETATION OF THIS ERROR CODE IS GIVEN ON PAGE 8 OF THIS
;PROGRAM.
;REGISTERS USED:
; R0 AND R1 PASS ARGUMENTS AND R0 IS ALTERED
; ALL FLOATING AC'S ARE GARBAGED
INTARM: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;SETUP BUTTON INTERRUPT HANDLER ROUTINE
SETVEC #ARMTRP,#ARMERR,#SUPST7 ;SET PANIC BUTTON TRAP ROUTINE
SETVEC #DRBTRP,#YERR,#SUPST7 ;SET YELLOW PANIC BUTTON TRAP ROUTINE
CLR KICDAC ;INDICATE NO DAC REFRESH JOB RUNNING
;RANDOM INITIALIZATION
CLR FINFLG ;WE'RE NOT DONE YET.
CLR ADCSR ;TURN OFF BLUE ADC INTERRUPTS
MOV #3,DR11S ;DISABLE INTERUPT B AND
TST DR11I ;CLEAR REQUEST B (YELLOW PANIC INTERUPT REQ)
CLR CTRDAT ;RESET CENTER DATA
CLR ACTNUM ;RESET TOUCH SENSOR EVENT DATA
MOV #TCHVAL,R3
MOV #LSTUSE-TCHVAL/2,R4
CLR (R3)+
SOB R4,.-2
;INITIALIZE FORCE SENSING/COMPLIANCE ROUTINES
MOV #FRCJOB,R2 ;FREE ALL FORCE JOB BLOCKS
MOV R2,FRCPTR ;FORM ONE BIG CHAIN OF LINKED BLOCKS
MOV #FRCBLK,R4 ;NUMBER OF BLOCKS TO LINK
1$: MOV R2,R3 ;LINK THEM
ADD #16,R2 ;EACH BLOCK IS 7 WORDS LONG
MOV R2,(R3)
SOB R4,1$
CLR (R3) ;ZERO MARKS END
MOV #FZAPS,R2 ;ZERO ALL REQUIRED FORCE WORDS
MOV #FZAPE-FZAPS/2,R3
CLR (R2)+
SOB R3,.-2
;CLEAR THE LOCKS ON NON-REENTRANT CODE
MOV #-1,STTLCK ;SETTH
MOV #-1,SRFLCK ;SETREF
MOV #-1,BASLCK ;SETBAS
MOV #-1,WSTLCK ;WRIST
CLR BOVLCK ;BOVERM LOCK
;CLEAR LEVEL 6 DEVICE STATUS TABLE
MOV #dvstat,R3
10$: CLR (R3)+
CMP #-1,(R3)
BNE 10$ ;Zero entries until we see a -1
;CLEAR LEVEL 6 SERVICE REQUEST TABLE
CLR GOSTH
CLR GOSRF
CLR GOOPR
CLR GOBAS
CLR GOWST
;SCHEDULE THE LEVEL 6 SERVO ROUTINE
SCHEDU #SVPDB,#SERVO,#USRDM,#16.
;CHECK WHICH JOINT SERVOS ARE OPERATIONAL
MOV #MAXSRV,R3 ;GET NUMBER OF EXISTING SERVOS TO CHECK
MOV #SERVOS,R0 ;GET PTR TO LIST OF EXISTING SERVOS
CLR R4 ;CONSTRUCT SERVO BITS IN HERE
CLR R5
2$: MOV (R0)+,R2 ;GET POINTER TO SERVO DATA BLOCK
ASHC #1,R4 ;MAKE ROOM FOR NEXT SERVO BIT
BIT #NONEX,(R2) ;CHECK IF JOINT OPERATIONAL
BNE 3$ ;BRANCH IF ITS NOT
BIS #1,R5 ;ELSE SET SERVO BIT FOR FURTHER CHECKING
JSR PC,SETSET ;INITIALIZE SERVO DATA VARIABLES
3$: SOB R3,2$ ;REPEAT FOR ALL EXISTING SERVOS
ASHC #32.-MAXSRV,R4 ;LEFT JUSTIFY SERVO BITS
MOV R4,INCOEF ;SAVE SERVO BITS IN LOCAL COEFFICENT LIST
MOV R5,INCOEF+2
;ATTACH REQUIRED SERVOS
MOV #INTBLK,R0 ;POINT TO PTR TO SERVO BIT LIST
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE AND SET UP
; DEVICE BLOCK
BCS INTDNE ;BRANCH IF ATTACH ERROR OCCURRED
TST R0 ;BRANCH IF AT LEAST ONE SERVO EXISTS
BGT 4$
MOV #WRGNUM,R0 ;ELSE SIGNAL ERROR
BR INTDNE
;READ CURRENT JOINT ANGLES, TACH ZERO, AND REFERENCE POWER SUPPLY
4$: JSR PC,SETREF ;CHECK REFERENCE POWER SUPPLY VOLTAGE AND
; ZERO VELOCITY TACH READINGS
BCS INTDNE ;BRANCH IF ANY ERROR OCCURRED
JSR PC,SETTH ;READ CURRENT JOINT ANGLES IN ALL JOINTS
BCS INTDNE ;BRANCH IF WIPER ERROR OCCURRED
CLR R0 ;INDICATE NO ERRORS
INTDNE: JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE THE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;RETURN
;INFORMATION FOR SERVO DATA BLOCKS
DATA
INTBLK: INCOEF ;POINTER TO COEF. DATA LIST
0 ;NOT A ARM MOTION FUNCTION
0
0
0
0
;LOCAL STORAGE
FINFLG: .WORD 0 ;FLAG TO SIGNAL SERVO (LVL6) TO BE KILLED
BOVLCK: .WORD 0 ;FLAG TO SIGNAL OVERRUN CONDITION
INCOEF: .WORD 0,0,0 ;SHORT COEFFICENT LIST USED TO ATTACH SERVOS
SVPDB: PDBLK 6,60,F ;PDB FOR LEVEL6 SERVO ROUTINE
CODE
;END OF "INTARM"
;FINARM - called by AL to kill servo level 6 job at end of user program
;Calls from AL start the servo level 6 job via INTARM and stop it via this
;routine, FINARM.
FINARM: MOV #1,FINFLG ;SET FLAG: SERVO WILL BE DISMISSED ON NEXT TICK
RTS PC ;RETURN
;END OF "FINARM"
;DRIVE - initiates motions for one joint, single segment
;A DIFFERENTIAL CHANGE IN JOINT ANGLE IS INITIATED FOR A SINGLE SERVO. THIS
;MODE IS INTENDED PRIMARY AS A DIAGNOSTIC TOOL TO CALIBRATE THE JOINT VARIABLES.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 4 WDS.
; JSR PC,DRIVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; ALL FLOATING AC'S ARE GARBAGED
.IFNZ DIAGY+CPOINTY ;*MSM
DRIVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
;ATTACH REQUIRED SERVO
MOV R0,DRVBLK ;SAVE POINTER TO COEFFICIENT DATA LIST
MOV R0,R4
MOV #DRVBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE,SET UP IN DEVICE
; BLOCK
BCS DRVERR ;BRANCH IF ATTACH ERROR OCCURRED
CMP #1,R0 ;CHECK THAT ONLY ONE JOINT HAS BEEN SELECTED
BEQ 1$ ;BRANCH IF ONLY ONE
MOV #WRGNUM,R0 ;SIGNAL ERROR
BR DRVERR ;EXIT
;CORRECT TRAJECTORY FOR INITIAL JOINT ANGLE
1$: ADD #DATST+A0COEF,R4;POINT TO CONSTANT TERM OF FIRST POLYNOMIAL
MOV 4(R1),R2 ;GET POINTER TO JOINT THAT IS TO BE SERVOED
MOV TH(R2),(R4) ;SET INITIAL VALUE OF POLY TO PRESENT JOINT ANGLE
MOV TH+2(R2),2(R4)
;CHECK IF FINAL JOINT ANGLE OUT OF RANGE
MOV #5,R3 ;ADD ALL SIX COEFFICIENTS
LDF (R4)+,AC0 ;LOAD CONSTANT TERM
ADDF (R4)+,AC0 ;ADD IN ALL OTHER COEFFICIENTS
SOB R3,.-2
CMPF USTOP(R2),AC0 ;COMPARE FINAL ANGLE TO JOINT UPPER STOP LIMIT
CFCC
BGE 3$ ;SKIP IF WITHIN RANGE
2$: MOV #4,R0 ;SIGNAL ERROR
BR DRVERR ;EXIT CLEANLY
3$: CMPF LSTOP(R2),AC0 ;COMPARE FINAL ANGLE TO JOINT LOWER STOP LIMIT
CFCC
BGT 2$ ;BRANCH IF NOT IN RANGE
;INITIATE SERVOS TO RUN
.IFNZ DIAGY
MOV #DBUF+2,DBUF ;SETUP DIAGNOSTIC BUFFER
.ENDC
.IFNZ TACCAL
CLR TTACH ;CLEAR ACCUMULATED TACH READING IF TACCAL MODE
CLR TTACH+2
CLR TACRDG ;CLEAR TOTAL NUMBER OF TACH READINGS TAKEN
CLR TACOFR ;ZERO TOTAL NUMBER OF READINGS OUT OF RANGE
MOV #BELL,SG
JSR PC,TYPSTR
.ENDC
MOV DRVBLK,R0 ;PTR TO COEF. LIST
JSR PC,RUNSRV ;GO RUN THE SERVOS
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
DRVERR: JSR PC,RELSRV ;RELEASE SERVOS IN DEVICE BLOCK
MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;INFORMATION FOR SERVO DATA BLOCKS
DATA
DRVBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
48. ;DELAY STARTING BY 48 MILLISECONDS
DATST ;POINTER TO START O F SEGMENT DATA
CODE
.ENDC
;END OF "DRIVE"
;CENTER - closes the hand fingers & centers the arm over any grasped object
;THE ORGANIZATION OF THE COEFFICIENT DATA ARRAY MUST BE AS FOLLOWS:
;
; COFLST: XXXXXX ;TWO SERVO BIT WORDS, 7 BITS MUST BE ON, A HAND
; XXXXXX ; SERVO AND ALL JOINT SERVOS OF THE SAME ARM
; 0 ;NO COMMAND BITS
; 0 ;DONT WOBBLE
; 0 ;NO NEXT SEGMENT
; 0 ;NO FUNCTION TIME
; 0 ;NO TRANSFORM
; CODE ;PTR TO CODE TO BE SCHEDULED THIS SEG
; ;NO POLYNOMIAL TO FOLLOW
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,CENTER
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 GARBAGED
CENTER: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;UPDATE CURRENT POSITION
MOV R0,-(SP) ;SAVE R0
MOV #CWRE,R0 ;POINT TO COEFF. LIST FOR WHERE
MOV #CDVL,R1 ;POINT TEMPORARY DEVICE LIST
JSR PC,WHERE
TST R0
BEQ 3$ ;BRANCH IF NO WHERE ERROR
3$: MOV (SP)+,R0
MOV (SP),R1 ;RESTORE R1
;ATTACH REQUIRED SERVOS
MOV R0,CNTBLK ;SAVE POINTER TO COEF. DATA LIST
MOV #CNTBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC .+6
JMP CNTDNE ;BRANCH IF ATTACH ERROR OCCURRED
CMP #7,R0 ;CHECK THAT EXACTLY 7 JOINTS ATTACHED
BEQ 1$
MOV #WRGNUM,R0 ;SIGNAL ERROR IF NOT THE RIGHT NUMBER OF JTS
JMP CNTDNE
;COMPUTE THE CHANGE IN JOINT ANGLES FOR A MOTION ALONG THE SWEEP DIRECTION
1$: MOV #YELARM,R2 ;ASSUME YELLOW ARM ATTACHED
BIT #YHAND,@CNTBLK ;CHECK WHICH HAND THIS IS
BNE .+6 ;SKIP IF IT IS THE YELLOW HAND
MOV #BLUARM,R2 ;ELSE INDICATE BLUE ARM
MOV #THPPTR,R1 ;GET POINTERS TO ALL JOINT ANGLES
MOV #CNTTRN,R0 ;STORE CURRENT ARM TRANSFORM HERE
JSR PC,UPDATE ;COMPUTE ARM TRANSFORM
LDF CNTTRN+12.,AC0 ;MODIFY THE POSITION BY THE ORIENTATION UNIT VECTOR
ADDF CNTTRN+36.,AC0
STF AC0,CNTTRN+36.
LDF CNTTRN+16.,AC0
ADDF CNTTRN+40.,AC0
STF AC0,CNTTRN+40.
LDF CNTTRN+20.,AC0
ADDF CNTTRN+44.,AC0
STF AC0,CNTTRN+44.
MOV #CNTTRN,R0 ;POINT TO NEW TRANSFORM
MOV #THPPTR,R1 ;PUT NEW JOINT ANGLES IN HERE
JSR PC,SOLVE ;COMPUTE NEW JOINT ANGLES FOR CHANGE
TST R0 ;CHECK IF SOLUTION EXISTS
BEQ 2$ ;BRANCH IF IT DOES
MOV #NOSOLU,R0 ;ELSE SIGNAL ERROR
BR CNTDNE
;SET THE CHANGE IN JOINT ANGLE IN ALL ARM JOINTS
2$: MOV #BSRVOS,R0 ;GET POINTER TO BLUE ARM SERVOS
BIT #BLUARM,R2 ;CHECK WHICH ARM WE ARE USING
BNE .+6
MOV #YSRVOS,R0 ;REPLACE WITH POINTER TO YELLOW ARM IF NECESSARY
MOV #7,R3 ;WANT TO SET DELTHD FOR ALL SIX JOINTS AND HAND
LDF JTRATE,AC1 ;LOAD THE % CHANGE IN JT ANGLE PER MILLISECOND
BR SETCTR
CNTLP: LDF THP(R1),AC0 ;LOAD FINAL PLANNED JOINT ANGLE
SUBF TH(R1),AC0 ;GET TOTAL DIFFERENTIAL CHANGE
MOV TH(R1),THP(R1) ;RESTORE PREDICTED POSITION
MULF AC1,AC0 ;GET CHANGE PER MILLISECOND
MOV TH+2(R1),THP+2(R1)
STF AC0,DELTHD(R1) ;SET DIFFERENTIAL VELOCITY
SETCTR: MOV (R0)+,R1 ;GET POINTER TO ARM SERVO DATA BLOCK
MOV TH(R1),DELTH(R1) ;INITIAL JOINT ANGLE SET TO PRESENT ANGLE
MOV TH+2(R1),DELTH+2(R1)
LDF NCI(R1),AC0 ;SET GRAVITY LOADING AND INERTIA EQUAL TO
STF AC0,CI(R1) ; INITIAL VALUES
LDF NCII(R1),AC0
STF AC0,CII(R1)
SOB R3,CNTLP ;DO ALL JOINTS
MOV #32.,TTIME(R1) ;ONLY DELAY STARTING HAND BY 32 MILL. SECONDS
MOV RATE1,DELTHD(R1) ;SET INITIAL RATE OF HAND CLOSING
MOV RATE1+2,DELTHD+2(R1)
;INITALIZE CENTER DATA AND START SERVOS
; mov #mainl+400,ctrptr ;init data gatherer
MOV #CTRBLU,R3 ;GET PTR TO BLUE ARM TOUCH SENSOR DATA BLOCK
BIT #BLUARM,R2 ;CHECK WHICH ARM WE ARE USING
BNE 1$
MOV #CTRYEL,R3 ;REPLACE WITH PTR TO YELLOW TOUCH SENSORS IF NEC
1$: CLR RTTRIG(R3) ;CLEAR RIGHT FINGER STATE
CLR LFTRIG(R3) ;CLEAR LEFT FINGER STATE
CLR NUMTCH(R3) ;BOTH TOUCH SENSORS OFF TO BEGIN WITH
BIS R2,CTRDAT ;INDICATE CENTERING FOR THIS ARM
MOV CNTBLK,R0 ;PTR TO COEF. LIST
MOV (SP),R1 ;PTR TO DEVICE BLOCK
JSR PC,RUNSRV ;GO RUN THE SERVOS
BIC R2,CTRDAT ;INDICATE THIS ARM NOT CENTERING
CMP #2,NUMTCH(R3) ;CHECK IF CENTER FUNCTION EXEC. PROPERLY
BNE CNTDNE ;EXIT IF NOT BOTH TOUCH SENSORS ACTIVIATED
MOV (SP),R1 ;ELSE CLEAR "TMEOUT" BIT FOR HAND, THIS IS
MOV 20(R1),R1 ; THE NORMAL END CONDITION
BIC #TMEOUT,(R1)
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
CNTDNE: MOV (SP)+,R1 ;RESTORE POINTER TO DEVICE BLOCK
JSR PC,RELSRV ;RELEASE ATTACHED SERVOS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR "CENTER"
DATA
CNTTRN: .BLKW 24. ;STORAGE AREA FOR ARM TRANSFORM
;INFORMATION FOR SERVO DATA BLOCKS
CNTBLK: 0 ;POINTER TO COEF. DATA LIST
15000. ;ALLOW 15 SECONDS FOR FUNCTION
16000. ;DELAY STARTING BY ALL JOINTS BUT HAND UNTIL TOUCH
0 ;NO POLYNOMIALS
CWRE: 177774 ;WHERE ARE ALL 7 JOINTS OF BLUE ARM and yellow arm *kc
0
CDVL: .BLKW 20 ;STORAGE FOR WHERE
CODE
;END OF "CENTER"
; CTRLV6 - center touch sensor monitor
;THIS ROUTINE IS CALLED BY SERVO IF THERE ARE ANY PENDING CENTER ACTIONS
;FOR CURRENT ARM. FINGERS ARE READ TO SEE IF TOUCH SENSOR TRIGGERED. IF
;ONLY ONE TOUCH SENSOR IS ON, THE SPEED AT WHICH THE HAND IS CLOSING IS
;REDUCED AND THE JOINT SERVOS OF THE ARM START RUNNING. THE GROSS MOTION
;OF THE ARM IS INTENDED TO NEGATE THE SPEED AT WHICH THE HAND IS CLOSING TO
;PRODUCE A NET VELOCITY OF ZERO FOR THE TOUCH SENSOR IN CONTACT WITH AN
;OBJECT. IF BOTH TOUCH SENSOR ARE NOW ON, ARM JOINT MOTION IS STOPPED AND
;THE HAND IS PERMITTED TO CLOSE EVENLY ABOUT THE OBJECT. THIS ROUTINE HAS
;TWO ENTRY POINTS WHICH ARE USED FOR DIFFERENTIATING BETWEEN TOUCH SENSORS.
;IT IS ASSUMED THAT WHEN EITHER OF THESE ROUTINES IS CALLED, REGISTER R5
;CONTAINS A POINTER TO EITHER THE "CTRYEL" OR THE "CTRBLU" DATA BLOCKS.
;DAT SHOULD POINT AT DEVICE BLOCK OF CURRENT ARM
CTRLV6: ADD #4,DAT ;POINT TO FIRST SERVO POINTER IN DEV BLOCK
MOV (DAT),R0 ;GET POINTER TO FIRST SERVO
bit CTRDAT,MECHSM(R0) ;ARE WE CENTERING THIS ARM?
BEQ MONDNE ;BR IF NOT
MOV #CTRBLU,R4 ;assume blue arm center
bit #YELARM,MECHSM(R0) ;ARE WE CENTERING YELLOW ARM?
BEQ 1$ ;BR IF NOT YELLOW
MOV #CTRYEL,R4 ;POINT TO YEL. CENTER DATA
1$: MOV RTCHN(R4),R0 ;CHECK RIGHT FINGER READING
JSR PC,FFORCE
; jsr pc,diagff
TST RTTRIG(R4) ;CHECK IF ALREADY TRIGGERED
BNE 2$
CMPF RTHRES(R4),AC0 ;CHECK TRIGGER THRESHOLD
CFCC
BGT 2$ ;BR IF NOT TRIGGERED
INC RTTRIG(R4) ;ELSE INDICATE RIGHT FING TRIGGERED
MOV #100000,R3 ;INDICATE RIGHT TOUCH
BR MONSTR
2$: MOV LFCHN(R4),R0 ;CHECK LEFT FINGER READING
JSR PC,FFORCE
; jsr pc,diagff
TST LFTRIG(R4) ;CHECK IF ALREADY TRIGGERED
BNE MONDNE
CMPF LTHRES(R4),AC0 ;CHECK TRIGGER THRESHOLD
CFCC
BGT MONDNE ;BR IF NOT TRIGGERED
INC LFTRIG(R4) ;ELSE INDICATE LEFT FING TRIGGERED
CLR R3 ;INDICATE LEFT TOUCH
;COMMON CODE FOR BOTH ENTRY POINTS
MONSTR: MOV #6,R2 ;FIX UP 6 ARM JOINTS
TST NUMTCH(R4) ;CHECK IF THIS IS THE FIRST TOUCH SENSOR ON
BNE 2$ ;IF SECOND TOUCH, GO STOP JOINT MOTION
;ONE TOUCH SENSOR ON, START JOINT MOTION
1$:; ldf oneone,ac0
; stf ac0,@ctrptr ;say one touch
; add #4,ctrptr
MOV (dat)+,R0 ;GET PTR TO JOINT SERVO
XOR R3,DELTHD(R0) ;INSURE JOINT GOING IN PROPER DIRECTION *K
MOV PTIME(R0),TTIME(R0) ;START JOINT MOVING
SOB R2,1$ ;REPEAT FOR ALL ATTACHED JOINTS
MOV (dat)+,R0 ;GET PTR TO HAND SERVO
MOV RATE2,DELTHD(R0) ;SLOW DOWN RATE OF HAND CLOSING
MOV RATE2+2,DELTHD+2(R0)
BR 3$ ;INDICATE 1 TOUCH SENSOR ON
;BOTH TOUCH SENSORS ON, STOP JOINT MOTION
2$:; ldf twotwo,ac0
; stf ac0,@ctrptr ;say two touch
; add #4,ctrptr
MOV (dat)+,R0 ;GET PTR TO JOINT SERVO
BIC #RUN,(R0) ;TELL JOINT TO STOP MOTION
SOB R2,2$ ;REPEAT FOR ALL ARM JOINTS, NO HAND
MOV (dat)+,R0 ;ALLOW HAND TO CLOSE FOR A FEW MORE MIL-SEC.
MOV #CTRSTP,COUNT(R0)
3$: INC NUMTCH(R4) ;INCREMENT THE NUMBER OF TOUCH SENSORS ON
;EXIT CLEANLY
MONDNE: RTS PC
;data
;ctrptr: .word 0
;oneone: .flt2 1111.0
;twotwo: .flt2 2222.0
;foo=mainl+400
;code
;diagff: RTS PC ;NOP TO GATHER FINGER FORCES
; stf ac0,@ctrptr ;store fforce value
; add #4,ctrptr
; rts pc
;END OF "MONCTR"
; Data for center operations
DATA
;RELATIVE POINTERS FOR "CTRYEL" AND "CTRBLU" DATA BLOCKS
rtchn ==0 ;right finger channel
lfchn ==2 ;left finger channel
rttrig ==4 ;right trigger state, 0 → not triggered yet
lftrig ==6 ;left trigger state
numtch ==10 ;number of touch sensors active: 0,1,2
rthres ==12 ;threshold on right finger
lthres ==16 ;threshold on left finger
CTRDAT: .WORD 0 ;CENTER STATUS WORD *k
;DATA FOR OPERATING WITH YELLOW ARM
CTRYEL: 4 ;yellow hand right finger #
5 ;yellow hand left finger #
0 ;right trigger state
0 ;left trigger state
0 ;BOTH TOUCH SENSORS INITIALLY OFF
.flt2 1.0 ;right threshold
.flt2 1.0 ;left threshold
;DATA FOR OPERATING WITH BLUE ARM
CTRBLU: 1 ;blue hand right finger #
0 ;blue hand left finger #
0 ;right trigger state
0 ;left trigger state
0 ;BOTH TOUCH SENSORS INITIALLY OFF
.flt2 400.0 ;right threshold
.flt2 400.0 ;left threshold
;RATES AT WHICH JOINTS AND HAND MOVE
RATE1: .FLT2 -.005 ;INITAL RATE OF HAND CLOSING
RATE2: .FLT2 -.0005 ;RATE OF HAND CLOSING AFTER TOUCH
JTRATE: .FLT2 .00025 ;DIFFERENTIAL CHANGE IN JT ANGLE/MSEC.
;ON MONITOR PROCESSOR DESCRIPTOR BLOCKS
YRTPDB: PDBLK 5,20,,CTRYEL
YLFPDB: PDBLK 5,20,,CTRYEL
BRTPDB: PDBLK 5,20,,CTRBLU
BLFPDB: PDBLK 5,20,,CTRBLU
CODE
;END OF "CENTER" DATA
;WHERE - updates the current JT angle and dynamic coef for servos
;THE SERVOS SPECIFIED IN THE COEFFICIENT LIST HEADER ARE ATTACHED AND THEIR
;JOINT ANGLE, GRAVITY LOADING, AND JOINT INERTIA VALUES ARE UPDATED IN THEIR
;RESPECTIVE SERVO DATA BLOCKS. ONLY THE FIRST TWO WORDS OF THE COEFFICIENT
;LIST ARE USED BY THIS ROUTINE.
;
; COFLST: XXXXXX ;TWO SERVO BIT WORDS
; XXXXXX
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK
; JSR PC,WHERE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0 IS GARBAGED
WHERE: MOV R0,WHRBLK ;SAVE POINTER TO COEFFICIENT DATA LIST
MOV #WHRBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE,SET UP IN DEVICE
; BLOCK
BCS 2$ ;EXIT IF ATTACH ERROR RETURNED
TST R0 ;BRANCH IF AT LEAST ONE SERVO ATTACHED
BGT 1$
MOV #WRGNUM,R0 ;SIGNAL ERROR
BR 2$ ;EXIT
;READ JOINT ANGLES AND UPDATE CURRENT JOINT INERTIA AND GRAVITY LOADING
1$: JSR PC,SETTH ;READ CURRENT JOINT ANGLES IN ALL ATTACHED SERVOS
BCS 2$ ;BRANCH IF WIPER ERROR OCCURRED
CLR R0 ;INDICATE NO PREPARATION ERROR
;EXIT CLEANLY, RELEASE ALL ATTACHED SERVOS
2$: JSR PC,RELSRV ;RELEASE SERVOS IN DEVICE BLOCK
RTS PC ;EXIT
;INFORMATION FOR SERVO DATA BLOCKS
DATA
WHRBLK: 0 ;POINTER TO COEF. DATA LIST
0 ;NOT A ARM MOTION FUNCTION
0
0
CODE
;END OF "WHERE"
;MOVE - moves the arm joints along a precomputed trajectory
;THE JOINT SERVOS SPECIFIED IN THE COEFFICIENT LIST HEADER ARE DRIVEN
;ACCORDING TO PREDETERMINED POLYNOMIALS REPRESENTING THE JOINT ANGLES OR
;EXTENTIONS. ANY SINGLE DEVICE (AN ARM JOINT OR HAND JOINT FROM THE
;"YELLOW" AND "BLUE" ARMS, OR A PUMA) CAN BE SPECIFIED AS THE DEVICE
;TO MOVE.
;
;THE ORGANIZATION OF THE COEFFICIENTS DATA ARRAY IS EXACTLY AS PRESENTED ON
;PAGE 10 OF THIS PROGRAM. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,MOVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE
;BLOCK. THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS
;SUCCESSFULLY ATTACHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE
;THIRD WORD OF THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE
;LOGICAL NUMBER OF ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN
;THAT SERVOS STATUS ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
.ifz short
.IFZ STDALN
MOVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
TST (R0) ;PUMA? (check servo bits)
BNE 20$ ; No
BIT #GHAND+RHAND,2(R0) ;Red or green hand?
BEQ 20$ ; No
PUSH R0 ;Save pointer to coefficient list
MOV #IOREG,R1 ;Now see if arm power is on - read status word
MOV #SRV17,DAT ;Assume green hand
CMP 2(R0),#GHAND ;Is it the green hand?
BEQ 23$ ;Yes - continue
MOV #SRV19,DAT ;set up for red hand
23$: GOSUB RSINGL ;Put status word into R0
POP R3 ;Retreive ptr to coeff. list, put into R3.
BIT #ISON,R0 ;Is "power on" bit set?
BNE 22$ ;If so, continue
MOV #NOPOWR,R0 ;Else complain
JMP MVEDNE
22$: MOV #AIRCLS,R0 ;Bit to close hand
TST 20(R3) ;Check C0 to see if we're to open=1 or close=0
BEQ 21$ ;Skip if closing hand
MOV #AIROPN,R0 ;Set bit to open hand
21$: BIC #AIROPN+AIRCLS,ARMBIT(DAT) ;Clear both bits.
GOSUB BITON ;Set bit in R0 on in joint 7 status word
SLEEP #500. ;Give it a chance to do it before proceding
CLR R0 ;Indicate no errors
JMP MVEDNE ;Return
;SET UP DATA ARRAY TO BE TRANSFERRED
20$: BIT #BARM,(R0) ;Blue arm?
BEQ 24$ ; No
MOV @FPSAV,GPTR ;GET POINTERS FOR DATA GATHERING
MOV @ISAV,GIPTR
24$:
;ATTACH REQUIRED SERVOS
MOV #3,AWAIT
MOV R0,MVEBLK ;SAVE POINTER TO COEF. DATA LIST
MOV R0,R3
1$: MOV #MVEBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC 2$ ;BRANCH IF NO ATTACH ERRORS
SLEEP #1 ;SLEEP AND TRY AGAIN
DEC AWAIT
BGT 1$
JMP MVEDNE ;ERROR RETURNED, SIGNAL ATTACH ERROR AND EXIT
2$: TST R0 ;BRANCH IF AT LEAST ONE JOINT SELECTED
BGT 3$
MOV #WRGNUM,R0 ;ELSE SIGNAL ERROR
JMP MVEDNE ;EXIT CLEANLY
;SET UP JOINT WOBBLING IF NECESSARY
3$: BIT #WOBBLE,OPBITS(R3) ;WOBBLING?
BEQ 5$ ;NO
LDF @WOBMG(R3),AC0 ;HERE'S WERE WE GET THE WOBBLE MAGNITUDE
BIT #YARM,(R3) ;WOBBLING YELLOW ARM?
BEQ 4$ ;NO
STF AC0,YWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV04+WOBPTR
CLR SRV05+WOBPTR
MOV #20,SRV06+WOBPTR
4$: BIT #BARM,(R3) ;WOBBLING BLUE ARM?
BEQ 5$ ;NO
STF AC0,BWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV11+WOBPTR
CLR SRV12+WOBPTR
MOV #20,SRV13+WOBPTR
;INITIATE THE SERVOS FOR RUNNING
5$: MOV (SP),R1 ;POINT TO DEVICE BLOCK
MOV MVEBLK,R0 ;PTR TO COEF LIST
JSR PC,RUNSRV
BIT #BARM,(R3) ;Blue arm? (need to turn off force system)
BEQ MVEDNE ; No
MOV R0,-(SP) ;SAVE ERROR BITS
TST GDATA ;WAS GATHER ENABLED?
BLE 6$ ;NO *k
MOV #30.,GCNT ;SAMPLE FOR 1/2 SECOND MORE
9$: JSR PC,REPS
JSR PC,RNS
SLEEP #16.
DEC GCNT
BGE 9$
; ASR GTIME ;COMPUTE NPTS for 30 hz sampling
MOV GIPTR,R1 ;GET INTEGER POINTER
MOV IDSAV,(R1)+ ;SAVE ID # *K
MOV GDATA,(R1)+ ;SAVE CONTROL BITS FOR GRAPH.SAI
MOV GTIME,(R1)+ ;SAVE NPTS FOR GRAPH.SAI
MOV R1,@ISAV ;REPLACE CHANGED POINTERS
MOV GPTR,@FPSAV
CLR GDATA
;KILL FRCLV6 JOB IF ACTIVE AND TAKE FORCE JOBS OUT OF QUEUE
6$: INCB FSTAT+1 ;LOCKOUT OTHER JOBS FROM FORCE DATA
BIS #FINAL,FSTAT ;SAY DIE
MOV #BLUARM,R1 ;CLEAR BLUE FORCE JOBS *Y
JSR PC,CLRFRC
DECB FSTAT+1 ;UNLOCK DATA
MOV (SP)+,R0 ;RESTORE ERROR BITS
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
MVEDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR "MOVE"
DATA
AWAIT: .WORD 3 ;WAIT TO ALLOW SERVOS TO BE RELEASED
;INFORMATION FOR SERVO DATA BLOCKS
MVEBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
32. ;DELAY STARTING BY 32 MILLISECONDS
DATST ;POINTER TO START OF SEGMENT DATA
CODE
.ENDC
;MOVE - diagnostic move instruction without trajectory modification
;THE ORGANIZATION OF THE COEFFICIENTS DATA ARRAY IS EXACTLY AS PRESENTED ON PAGE
;10 OF THIS PROGRAM. A PLANNED TRAJECTORY THAT IS INVALID DUE TO A CHANGE
;IN THE START OR END POINTS OF ANY OF ITS SEGMENTS IS CORRECTED BY THIS ROUTINE.
;TRAJECTORY MODIFICATION IS ACCOMPLISHED BY ADDING FIFTH ORDER POLYNOMIALS TO
;THE PLANNED POLYNOMIALS. CORRECTIONS ARE ALWAYS APPLIED BETWEEN VIA POINTS.
;IF TWO VIA POINTS HAVE ONE OR MORE FREE POINTS BETWEEN THEM, ALL OF THE SEGMENTS
;BETWEEN THE VIA POINTS ARE TREATED AS ONE SINGLE SEGMENT AND THE FIFTH ORDER
;CORRECTION POLYNOMIAL IS APPLIED ACROSS THIS SINGLE LONG SEGMENT.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,MOVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. IN ADDITION, IF NO ERRORS OCCUR DURING
;PREPARATION, R1 RETURNS THE NUMBER OF SERVOS ATTACHED AND EACH WORD OF THE
;DEVICE BLOCK CONTAINS THE LOGICAL NUMBER OF ONE OF THE ATTACHED SERVOS IN
;ITS LOWER BYTE AND THAT SERVOS STATUS ERROR BITS IN THE UPPER BYTE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
.IFNZ STDALN
MOVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;ATTACH REQUIRED SERVOS AND DETERMINE NUMBER OF JOINTS AND ARMS TO BE RUN
MOV R0,MVEBLK ;SAVE POINTER TO COEF. DATA LIST
MOV R0,R3
MOV #MVEBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC 1$ ;BRANCH IF NO ATTACH ERRORS
JMP MVEDNE ;ERROR RETURNED, EXIT CLEANLY
1$: MOV R0,MVSRCT ;SAVE NUMBER OF ATTACHED SERVOS
;SET UP JOINT WOBBLING IF NECESSARY
BIT #WOBBLE,OPBITS(R3) ;WOBBLING?
BEQ 3$ ;NO
LDF @WOBMG(R3),AC0 ;HERE'S WERE WE GET THE WOBBLE MAGNITUDE
BIT #YARM,(R3) ;WOBBLING YELLOW ARM?
BEQ 2$ ;NO
STF AC0,YWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV04+WOBPTR
CLR SRV05+WOBPTR
MOV #20,SRV06+WOBPTR
2$: BIT #BARM,(R3) ;WOBBLING BLUE ARM?
BEQ 3$ ;NO
STF AC0,BWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV11+WOBPTR
CLR SRV12+WOBPTR
MOV #20,SRV13+WOBPTR
;GET POINTER TO FIRST SEGMENT DATA AND DETERMINE IF THE STARTING POINT IS
;WITHIN JOINT ANGLE ERROR TOLERANCE
3$: MOV MVEBLK,R0 ;GET POINTER TO START OF COEF. DATA LIST
ADD #DATST+A0COEF,R0 ;ADDR OF 1ST A0 COEFFICIENT
MOV MVSRCT,R3 ;GET THE NUMBER OF JOINT SERVOS ATTACHED
ADD #4,R1 ;POINT TO THE FIRST SERVO DATA BLOCK
MVCMPT: MOV (R1)+,R4 ;GET ADDRESS OF SERVO BLOCK DATA
LDF TH(R4),AC0 ;LOAD THE CURRENT JOINT ANGLE
SUBF (R0),AC0 ;COMPUTE THE ERROR IN POSITION, TH-A0
STF AC0,AC1
ABSF AC0
CMPF ERRTOL(R4),AC0 ;COMPARE TO ERROR TOLERANCE
CFCC
BGE INTOL ;SKIP IF WITHIN TOLERANCE
MOV R0,R2 ;GET THE ADDR OF THE POLYNOMIAL TO MODIFY
MOV TH(R4),(R2)+ ;SET START POINT EQUAL TO CURRENT JT ANGLE
MOV TH+2(R4),(R2)+
NEGF AC1 ;HAVE TO CHANGE BY THIS MUCH TO GET BACK
MOV #A15TH,R4 ;GET ADDRESS OF CORRECTION POLYNOMIAL
MOV #5,R5 ;CORRECT NEXT 5 COEFFICIENTS
1$: LDF (R4)+,AC0 ;GET NORMALIZED COEFFICIENT
MULF AC1,AC0 ;COMPUTE TOTAL CHANGE
ADDF (R2),AC0 ;APPLY CORRECTION
STF AC0,(R2)+ ;AND SAVE
SOB R5,1$
INTOL: ADD #A5COEF-A0COEF+4,R0 ;POINT TO NEXT JOINT STARTING ANGLE
SOB R3,MVCMPT ;DO FOR ALL ATTACHED SERVOS
;INITIATE THE SERVOS FOR RUNNING
MOV (SP),R1 ;POINT TO DEVICE BLOCK
MOV MVEBLK,R0 ;PTR TO COEF. LIST
JSR PC,RUNSRV
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
MVEDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR MOVE
DATA
MVEBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
32. ;DELAY STARTING BY 32 MILLISECONDS
DATST ;POINTER TO START OF SEGMENT DATA
MVSRCT: 0 ;NUMBER OF ATTACHED JOINTS
CODE
.ENDC
.endc
;ATTSRV - routine for attaching servos
;"ATTSRV" FILLS THE DEVICE LIST WITH THE ADDRESSES OF THE SERVOS INDICATED
;BY THE SERVO BITS IN THE COEFFICIENT LIST HEADER. THE ADDRESS OF THE
;DEVICE LIST IS PLACED IN EACH SERVOS "INUSE" DATA BLOCK WORD TO INDICATE
;THAT THE SERVO IS ATTACHED TO A DEVICE. IF "INUSE" IS NOT INITIALLY ZERO
;IN ANY SERVO DATA BLOCK THAT IS TO BE ATTACHED, AN ERROR RETURN IS TAKEN.
;"DATPT", "POLY", "FCI", "TTIME", "COUNT" AND THE "MODBTS" WORD IN EACH SERVO
;DATA BLOCK ARE ALSO INITIALIZED. If the device is a puma, different words
;in the PUMA data block are initialized. A SAMPLE CALL FOLLOWS:
;
; MOV #CBLK,R0 ;POINTER TO INFORMATION BLOCK
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,ATTSRV
; BCS ERROR ;CHECK FOR ERRORS
;WHERE
; CBLK: COEF ;POINTER TO COEFFICIENT LIST HEADER
; CNT ;MAXIMUM FUNCTION TIME FOR TIME OUT
; WAIT ;SERVO START WAIT TIME
; SEG ;REL. PTR. TO START OF SEGMENT DATA, 0 IF NO POLYNOMIALS
;
;THE DEVICE BLOCK IS LEFT IN THE FOLLOWING FORM:
;
; DEVICE: .BYTE NUM,0 ;WHERE NUM IS THE NUMBER OF SERVOS ATTACHED
; 0 ;LEAVES A BLANK TO PUT THE EVENT NUMBER IN
; SERVO-1 ;PTRS TO SERVO DATA BLOCKS
; SERVO-2
; :
; SERVO-NUM
;
;AFTER EXECUTION, IF AN ATTACH ERROR OCCURRED, THE C BIT IS SET AND R0 CONTAINS
;THE ERROR CODE "CNTATT", OTHERWISE, R0 CONTAINS THE NUMBER OF DEVICES ATTACHED.
;DEFINITIONS:
SETCNT==2
SETSWT==4
RELSEG==6
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
ATTSRV: MOV R5,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R3,-(SP)
MOV R2,-(SP)
MOV R1,-(SP) ;SAVE DEVICE BLOCK ADDRESS
MOV (R0),R4 ;GET ADDRESS OF COEF. LIST
MOV (R4),R2 ;GET THE SERVO BITS TO DETERMINE # OF REQ. SRV.
MOV 2(R4),R3 ;32 POSSIBLE SERVOS
CLR R5
TST R2 ;CHECK IF SERVO #1 REQUESTED
1$: BPL .+4 ;SKIP IF THIS SERVO NOT REQUESTED
INC R5 ;ELSE INCREMENT SERVO COUNT
ASHC #1,R2 ;GET NEXT SERVO BIT
BNE 1$ ;REPEAT TILL ALL SERVO BITS ADDED
MUL #A5COEF-A0COEF+4,R5 ;COMPUTE OFFSET TO START OF DYNAMIC COEF.
MOV R5,SETFCI ;SAVE OFFSET TO CII AND CI COEFFICIENTS
MOV OPBITS(R4),SETMOD ;GET THE COMMAND MODE BITS
MOV #A5COEF,SETPLY ;GET INITIAL REL PTR TO POLY DATA
MOV RELSEG(R0),SETSEG ;GET REL. PTR TO START OF SEGMENT DATA
BEQ 2$ ;BRANCH IF NO POLYNOMIAL TO FOLLOW
ADD (R0),SETSEG ;CONVERT SEG PTR TO ABSOLUTE ADDRESS
ADD SETSEG,SETPLY ;CONVERT POLY PTR TO ABSOLUTE ADDRESS
ADD SETSEG,SETFCI ;CONVERT DYNA. COEF. PTR TO ABS. ADDR.
ADD #A0COEF,SETFCI
2$: ADD #4,R1 ;START STORING SERVO PTRS. IN HERE
MOV (R4),R2 ;GET THE SERVO BITS AGAIN
MOV 2(R4),R3 ;32 POSSIBLE SERVOS
MOV #SERVOS,R4 ;ADDR. OF TABLE CONTAINING SERVO DATA BLK PTRS
MOV (R4)+,R5 ;GET THE POINTER TO THE SERVO DATA BLOCK
CLR @(SP) ;ZERO SERVO COUNTER
TST R2
ATTLP: BPL NXTSRV ;BRANCH IF THIS SERVO IS NOT TO BE USED
TST INUSE(R5) ;CHECK IF SERVO ATTACHED TO ANOTHER DEVICE
BEQ NOTIUS ;BRANCH IF NOT ALREADY ATTACHED
ATTERR: MOV #CNTATT,R0 ;SHOULDN'T BE ATTACHED, SIGNAL ERROR
SEC
BR ATTDNE
NOTIUS: TST (R5) ;CHECK STATUS WORD OF DEVICE
BNE ATTERR ;ALL STATUS BITS SHOULD BE OFF
MOV (SP),INUSE(R5) ;PUT DEVICE ADDR. IN SERVO BLOCK TO INDICATE
; SERVO IS ATTACHED
; bit #VIS+SCRE,MECHSM(R5) ;SKIP SETTING IF VSE OR SCRE *K
; BNE 3$
1$: bit #grnhnd+redhnd,mechsm(r5);don't init. anything for the puma hands
bne 3$
MOV SETSEG,DATPT(R5) ;SET POINTER TO SEGMENT DATA
MOV SETMOD,MODBTS(R5) ;SET THE OPERATION MODE BITS
MOV SETFCI,FCI(R5) ;SET PTR TO DYNAMIC COEF.
bit #grnarm+redarm,mechsm(r5) ;if we are setting up a puma, we must
beq 10$ ; do different things
clr ttime(r5)
clr badpjt(r5)
mov setply,ppoly1(r5) ;set ptr to polynomial coef joint #1
add #24.,setply ;increment ptrs to next block of coef
mov setply,ppoly2(r5) ;set ptr to polynomial coef joint #2
add #24.,setply ;increment ptrs to next block of coef
mov setply,ppoly3(r5) ;set ptr to polynomial coef joint #3
add #24.,setply ;increment ptrs to next block of coef
mov setply,ppoly4(r5) ;set ptr to polynomial coef joint #4
add #24.,setply ;increment ptrs to next block of coef
mov setply,ppoly5(r5) ;set ptr to polynomial coef joint #5
add #24.,setply ;increment ptrs to next block of coef
mov setply,ppoly6(r5) ;set ptr to polynomial coef joint #6
br 2$
10$: MOV SETSWT(R0),TTIME(R5) ;SET START WAIT TIME
MOV SETCNT(R0),COUNT(R5) ;SET COUNT FOR MAXIMUM FUNCTION TIME
MOV SETPLY,POLY(R5) ;SET PTR TO POLYNOMIAL COEF
2$: ADD #24.,SETPLY ;INCREMENT PTRS TO NEXT BLOCK OF COEF
ADD #10,SETFCI
3$: MOV R5,(R1)+ ;PUT SERVO DATA ADDR. IN DEVICE LIST
INCB @(SP) ;ADD 1 TO THE SERVO COUNT
NXTSRV: MOV (R4)+,R5 ;POINT TO NEXT SERVO DATA BLOCK
ASHC #1,R2 ;GET NEXT SERVO BIT
BNE ATTLP ;REPEAT IF SOME SERVO BITS LEFT
CLR (R1) ;PUT A 0 AT THE END OF THE DEVICE BLOCK
MOV @(SP),R0 ;RETURN NUMBER OF SERVOS ATTACHED
CLC ;INDICATE NO ATTACH ERRORS
ATTDNE: MOV (SP)+,R1 ;RESTORE REGISTERS
MOV (SP)+,R2
MOV (SP)+,R3
MOV (SP)+,R4
MOV (SP)+,R5
RTS PC ;NORMAL RETURN
;LOCAL STORAGE AREA FOR "ATTSRV"
DATA
SETMOD: 0 ;OPERATION MODE BITS TO BE PUT IN MODBTS WORD
SETPLY: 0 ;POINTER TO START OF POLYNOMIAL COEF.
SETFCI: 0 ;POINTER TO DYNAMIC COEFFICIENTS, CII & CI
SETSEG: 0 ;POINTER TO SEGMENT DATA
CODE
;END OF "ATTSRV"
;RELSRV - routine for detaching servos
;"RELSRV" RELEASES ALL SERVOS POINTED TO BY THE DEVICE BLOCK BY ZEROING
;INUSE WORDS AND STATUS WORD RUN ERROR FLAGS IN THE SERVO DATA BLOCKS. THE
;POINTERS TO THE ATTACHED SERVOS IN THE DEVICE BLOCK ARE REPLACED BY THE
;LOGICAL NUMBER OF THE SERVO AND ITS ASSOCIATED RUN ERROR BITS. THE SERVO
;NUMBER IS PLACED IN THE LOWER BYTE AND THE ERROR BITS IN THE UPPER BYTE OF
;THE DEVICE BLOCK WORD. ALL OF THE SERVO RUN ERROR BITS ARE OR'ED WITH
;REGISTER R0. A SAMPLE CALL FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,RELSRV
; TST R0 ;CHECK FOR RUN ERROR
;
;AFTER EXECUTION, THE FIRST WORD OF THE DEVICE BLOCK CONTAINS THE NUMBER
;OF SERVOS INITIALLY ATTACHED.
;REGISTERS USED:
; R0, R1 PASS ARGUMENTS AND R0 IS ALTERED
RELSRV: MOV R3,-(SP) ;SAVE REGISTERS
MOV R2,-(SP)
MOV R1,-(SP)
MOV R0,-(SP)
MOVB (R1),R3 ;GET THE NUMBER OF SERVOS ATTACHED
MOV R3,@2(SP) ;LEAVE NUMBER IN FIRST WORD OF DEVICE BLK
ADD #4,R1 ;SERVO POINTERS START HERE
TST R3 ;CHECK IF ANY SERVOS LEFT TO DETACH
1$: BLE 4$ ;BRANCH IF END OF DEVICE LIST
MOV (R1),R2 ;GET THE ADDR. OF A SERVO DATA BLOCK
BIS (R2),R0 ;OR ALL RUN ERROR BITS TOGETHER
CMP R2,#SRV17 ;Is it a PUMA?
BHIS 2$ ;Yes
MOVB SRVNUM(R2),(R1)+ ;SEND BACK SERVO NUMBER
BR 3$
2$: MOVB BADPJT(R2),(R1)+ ;Send back all the bad PUMA joints
3$: MOVB 1(R2),(R1)+ ;SEND BACK RUN ERROR BITS
JSR PC,SETSET ;RESET ALL JOINT VARIABLES
DEC R3 ;DECREMENT NUMBER OF SERVOS LEFT TO DETACH
BR 1$ ;GO CHECK IF MORE SERVOS TO DO
4$: BIC #377,R0 ;ISOLATE SERVO RUN ERROR BITS
BIS (SP)+,R0 ;OR ON PREVIOUS ERROR BITS
MOV (SP)+,R1 ;RESTORE DEVICE BLOCK POINTER
MOV (SP)+,R2
MOV (SP)+,R3
RTS PC ;RETURN
;END OF "RELSRV"
;SETSET - resets servo block variables after a move is completed
SETSET: CLR INUSE(R2) ;CLEAR IN USE FLAG, DETACH SERVO
CLR PTIME(R2) ;Next motion starts from t=0
CMP R2,#SRV17 ;Is it a PUMA?
BHIS 2$ ;Yes - most stuff doesn't exist so don't clear it
CLRF THD(R2) ;SET ALL VELOCITIES TO ZERO
CLRF THDP(R2)
MOV TH(R2),THP(R2) ;SET PREDICATED JOINT ANGLE TO ACTUAL
MOV TH+2(R2),THP+2(R2)
CLRF DELTH(R2) ;NO DEVIATION FROM TRAJECTORY AT END
CLRF DELTHD(R2) ;NO FINAL CONSTANT VELOCITY
CLRF ERRINT(R2) ;ZERO INTEGRAL OF POSITION ERROR
CLRF POSERR(R2) ;NO FINAL POSITION ERROR
CLRF VELERR(R2) ;NO FINAL VELOCITY ERROR
CLRF ERRTQE(R2) ;ZERO ERROR TORQUE
CLRF YOLD(R2) ;ZERO FORCE FILTER TERM
CLRF UOLD(R2) ;ZERO FORCE FILTER TERM
bit #BARM+BHAND,MECHSM(R2) ;SELECT INTERFAACE
BEQ 1$
CLRB DACVAL(R2) ;ZERO BLUE DAC OUTPUT
BR 2$
1$: BIC #7777,DACCHN(R2) ;ZERO YEL. DAC OUTPUT
2$: BIC #-1#NONEX,(R2) ;CLEAR STATUS WORD ERROR BITS
BIC #NNUL+DEPTPT+WOBBLE,MODBTS(R2) ;RESET MODE WORD
RTS PC
;END OF "SETSET"
;SETTH - reads A/D and initializes joint angles for a given device block
;LEVEL 6 CODE IS INITIATED TO READ THE REQUIRED JOINT ANGLES FROM THE ADC.
;THE LEVEL 6 CODE IS REPEATED UNTIL ALL OF THE WIPERS ARE WITH IN OPERATING
;RANGE OR UNTIL 4 ATTEMPTS HAVE BEEN MADE, WHICHEVER OCCURS FIRST. IF AT
;THE END SOME WIPERS ARE STILL OUT OF RANGE, AN ERROR RETURN IS TAKEN. "TH",
;THE ACTUAL JOINT ANGLE, AND "THP", THE PREDICTED JOINT ANGLE ARE INITIALIZED
;TO THE SAME VALUE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,SETTH
; BCS ERROR ;BRANCH IF ERROR DURING SETTH
;
;IF A WIPER ERROR OCCURS, THE C BIT IS SET AND R0 CONTAINS THE ERROR CODE
;"BADWIP" WHEN THIS ROUTINE EXITS. IF NO WIPER ERROR OCCURRED, NEW DYNAMIC
;COEFFICIENTS ARE COMPUTED FOR ANY ARM THAT HAD ANY OF ITS JOINT ANGLES
;UPDATED.
;REGISTERS USED:
; R1 PASSES ARGUMENTS AND IS NOT MODIFIED
; R0, AC0 ARE GARBAGED
SETTH: MOV R1,-(SP) ;SAVE REGISTERS
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
1$: INC STTLCK ;CHECK IF LOCKED OUT
BEQ 2$ ;BRANCH IF FREE TO GO ON
DEC STTLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR 1$
2$: MOV #STTSRV,R2 ;PUT LIST OF SERVOS WITH POTS TO READ IN HERE
MOVB (R1),R4 ;GET NUMBER OF SERVOS ATTACHED
BEQ STTDNE ;EXIT CLEANLY IF THERE ARE NONE
ADD #4,R1 ;SERVO POINTERS START HERE
CHKPOT: MOV (R1)+,R0 ;GET POINTER TO SERVO DATA BLOCK
CMP R0,#SRV17 ;Is it a PUMA?
BHIS 3$ ;Yes
TST POTCHN(R0) ;CHECK IF SERVO HAS A POT TO READ
BMI 1$ ;SKIP IF NO POT
3$: MOV R0,(R2)+ ;SAVE SERVO POINTER
1$: SOB R4,CHKPOT ;GO CHECK NEXT POT
CLR (R2) ;INDICATE END OF SERVO LIST
TST STTSRV ;CHECK IF ANY POTS TO READ
BEQ STTDNE ;EXIT CLEANLY IF THERE ARE NONE
MOV #4,STTCNT ;EXECUTE LEVEL6 CODE AT MOST 4 TIMES
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),STTEVT ;SAVE THE EVENT NUMBER
MOV #1,GOSTH ;REQUEST LVL 6 CODE BE RUN BY SERVO
EVWAIT (SP) ;WAIT FOR THE LEVEL 6 JOB TO FINISH
EVKIL ;(GOSTH CLEARED IN STHLV6)
TST STTWIP ;CHECK IF ALL WIPERS IN RANGE
BEQ 2$ ;BRANCH IF WIPERS OK
SEC ;INDICATE ERROR
MOV #BADWIP,R0
BR STTERR ;EXIT CLEANLY
;CONVERT THE A/D READINGS TO JOINT ANGLES FOR ALL SERVOS WITH POTS
2$: MOV #STTSRV,R4 ;SET POINTER TO LIST OF SERVOS WITH POTS
CLRB -(SP) ;COLLECT SERVO MECHANISM BITS IN HERE
STTLP: MOV (R4)+,R5 ;GET POINTER TO SERVO BLOCK DATA
BEQ COMPCI ;BRANCH IF END OF LIST
CMP R5,#SRV17 ;Is it a PUMA?
BHIS STTLP ;Yes - try next
BIS MECHSM(R5),(SP) ;OR ALL MECHANISM BITS TOGETHER
;** this used to be a bisb operation. might be bad
MOV POT(R5),R0 ;GET THE POT READING
bit #BLUARM+BLUHND,MECHSM(DAT) ;check if blue arm
BEQ 1$
ADD #4000,R0 ;SHIFT POT READING TO 0→7777 IF BLUE ARM
1$: ;don't shift if yellow
;ADD CORRECTION FOR NON-LINEAR POT ELEMENT AND CONVERT TO FLOATING POINT
.IFZ ISLIN
MOV R0,R3 ;SAVE NORMALIZED POT READING
CLR R1
ASHC #-10,R0 ;GET 4 MSB TO USE AS INDEX INTO NON-LINEAR TABLE
ROR R1 ;LEAVE 8 LSB AS POSITIVE TWOS COMPLEMENT NUMBER
ADD R5,R0 ;COMPUTE A POINTER INTO THE NON-LINEAR CORR. TABLE
MOVB NONLIN(R0),R2 ;GET LOWER LIMIT OF CALIBRATION TABLE
MOVB NONLIN+1(R0),R0 ;GET UPPER LIMIT TO INTERPOLATE
SUB R2,R0 ;GET DIFFERENCE
MUL R1,R0 ;INTERPOLATE
ASHC #1,R0 ;GET 16 MSB
ADD R2,R0 ;NOW HAVE PROPER CALIBRATION
ADD R3,R0 ;ADD TO NORMALIZED POT READING
.ENDC
LDCIF R0,AC0 ;CONVERT TO FLOATING POINT
MULF SCALE(R5),AC0 ;CONVERT FROM A/D UNITS TO JOINT ANGLE
ADDF OFFSET(R5),AC0 ;ADD POT OFFSET
STF AC0,TH(R5) ;SAVE THE JOINT ANGLE WITH THE SERVO DATA
STF AC0,THP(R5) ;SAVE AS PREDICTED JOINT ANGLE
BR STTLP ;GO DO NEXT SERVO
;RECOMPUTE DYNAMIC COEFFICIENTS IF ANY ARM JOINT ANGLES READ
COMPCI: MOVB (SP)+,R3 ;GET MECHANISM BITS FOR ALL SERVOS UPDATED
BIT #YELARM,R3
BEQ 1$ ;BRANCH IF NOT
MOV #YTH,R0 ;COMPUTE DYNAMIC COEF, BASED UPON JT. ANGS.
MOV #YNCI,R1 ;SAVE IN SERVO DATA BLOCKS
MOV #YELARM,R2 ;INDICATE YELLOW ARM
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
1$: BIT #BLUARM,R3 ;CHECK IF BLUE ARM JOINT ANGLES UPDATED
BEQ STTDNE ;BRANCH IF NOT
MOV #BTH,R0 ;ELSE COMPUTE CURRENT DYNAMIC COEF
MOV #BNCI,R1 ;UPDATE SERVO DATA BLOCKS
MOV #BLUARM,R2 ;INDICATE BLUE ARM
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
;EXIT CLEANLY
STTDNE: CLC ;INDICATE NO ERROR
STTERR: DEC STTLCK ;RELEASE CODE FOR FUTURE USE
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC
;LOCAL STORAGE AREA
DATA
STTLCK: -1 ;-1 → SETTH READY FOR USE, 0 → LOCKED UP
STTEVT: .BLKW 1 ;LEVEL 6 CODE COMPLETION EVENT
STTCNT: 4 ;NUMBER OF PASSES PERMITTED THROUGH LEVEL 6 CODE
STTWIP: 0 ;WIPER ERROR CODE CORRESPONDING TO "OUTRGE"
STTSRV: .BLKW 37. ;LIST OF SERVOS WITH POTS
CODE
;END OF "SETTH"
;SETREF - read A/D, set reference voltage and zero tach value readings
;LEVEL 6 CODE IS INITIATED TO READ THE REFERENCE VOLTAGE SUPPLY AND ANY
;TACHOMETER CHANNELS THAT MAY EXIST FOR THE SERVOS TO BE USED. A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,SETREF
;
;THE REFERENCE VOLTAGE READING AND THE TACHOMETER ZERO READINGS ARE READ
;AND AVERAGED FOUR TIMES TO REDUCE THE NOISE LEVEL. IF ANY OF THE READINGS
;DEVIATES SIGNIFICANTLY FROM THE PREVIOUS AVERAGE, AN ERROR CODE IS RETURNED
;IN R0, OTHERWISE R0 IS SET TO ZERO.
;REGISTERS USED:
; R1 PASSES ARGUMENT AND IS UNALTERED
; R0 RETURNS AN ERROR CODE
SETREF: MOV R1,-(SP) ;SAVE REGISTERS AND PTR TO DEVICE BLOCK
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
1$: INC SRFLCK ;CHECK IF LOCKED OUT
BEQ 2$ ;BRANCH IF FREE TO GO ON
DEC SRFLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR 1$
2$: MOV #SRFSRV+4,R2 ;SET POINTER TO LIST OF SERVOS WITH TACHS,SKIP
; OVER POWER SUPPLY REFERENCE READINGS
MOV #SRFADL+4,R3 ;SET POINTER TO A/D LIST, SKIP OVER REF. CHANNELS
;*K WE NEED MORE REF CHANNEL SPACE FOR YELLOW INTERFACE
MOVB (R1),R4 ;GET NUMBER OF SERVOS ATTACHED
BEQ ALLTAC ;BRANCH IF NO SERVOS ATTACHED
ADD #4,R1 ;POINT TO FIRST SERVO DATA BLOCK PTR IN DEVICE BLK
CHKTAC: MOV (R1)+,R0 ;GET PTR TO SERVO DATA BLOCK
CMP R0,#SRV17 ;Is it a PUMA?
BHIS 1$ ;Yes
TST TACCHN(R0) ;CHECK IF SERVO HAS A TACHOMETER
BMI 1$ ;SKIP IF NO TACH
MOV R0,(R2)+ ;SAVE SERVO POINTER
MOV TACCHN(R0),(R3)+ ;SAVE POT CHANNEL IN A/D LIST
MOV TACHZ0(R0),TACH(R0) ;SAVE PREVIOUS TACH ZERO READING
1$: SOB R4,CHKTAC ;REPEAT IF ANY SERVOS LEFT
ALLTAC: MOV #-1,(R3) ;INDICATE END OF A/D LIST
CLR (R2) ;INDICATE END OF SERVO LIST
MOV #10.,SRFCNT ;AVERAGE ADC READINGS ten TIMES
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),SRFEVT ;SAVE EVENT NUMBER FOR LEVEL 6 CODE
MOV #1,GOSRF ;REQUEST LVL6 CODE BE RUN BY SERVO
EVWAIT (SP) ;WAIT FOR THE LEVEL 6 JOB TO FINISH
EVKIL ;(GOSRF CLEARED IN SRFLV6)
;SECTION TO CHECK IF READINGS WITHIN TOLERANCE, START WITH REFERENCE
MOV REFVT1,R0 ;GET NEW REFERENCE VOLTAGE READINGS
SUB REFER1,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
BGT .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
BLE 1$ ;BRANCH IF REFERENCE OUT OF RANGE
MOV REFVT2,R0 ;REPEAT FOR SECOND REFERENCE READING
SUB REFER2,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
BGT .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
BGT 2$ ;BRANCH IF REFERENCE IN RANGE
; BLE 1$ ;BRANCH IF REFERENCE OUT OF RANGE
; MOV REFVT3,R0 ;CHECK YELLOW REFERENCE READINGS
; SUB REFER3,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
; BGT .+4 ;TAKE ABSOLUTE VALUE
; NEG R0
; CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
; BLE 1$ ;BRANCH IF REFERENCE OUT OF RANGE
; MOV REFVT4,R0 ;REPEAT FOR YELLOW REFERENCE READINGS
; SUB REFER4,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
; BGT .+4 ;TAKE ABSOLUTE VALUE
; NEG R0
; CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
; BGT 2$ ;BRANCH IF REFERENCE IN OF RANGE
1$: MOV #BADREF,R0 ;ELSE SIGNAL ERROR
SEC
BR SEFDNE ;EXIT
2$: MOV #SRFSRV+4,R3 ;GET POINTERS TO SERVOS WITH TACHS
3$: MOV (R3)+,R4 ;GET SERVO DATA BLOCK ADDRESS
BEQ TACSOK ;BRANCH IF END OF LIST
MOV TACHZ0(R4),R0 ;GET TACH. ZERO VELOCITY READING
SUB TACH(R4),R0 ;COMPUTE DIFF. FROM PREVIOUS VALUE
BPL .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #10,R0 ;CHECK IF WITH LIMITS
BGE 3$ ;GO CHECK NEXT ONE IF OK
MOV #BADTAC,R0 ;ELSE SIGNAL ERROR
SEC
BR .+4 ;EXIT
TACSOK: CLC ;SIGNAL NO ERROR
;SECTION TO EXIT FROM "SETREF"
SEFDNE: DEC SRFLCK ;RELEASE CODE FOR FUTURE USE
MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC ;RETURN
;LOCAL STORAGE AREA
DATA
SRFLCK: -1 ;-1 → SETREF READY FOR USE, 0 → LOCKED UP
SRFCNT: .BLKW 1 ;EXECUTION COUNTER FOR LEVEL 6 CODE
SRFEVT: .BLKW 1 ;LEVEL 6 CODE COMPLETION EVENT
SRFSRV: REFVT1-TACHZ0 ;STORAGE LOCATION FOR REF. POWER SUPPLY READINGS
REFVT2-TACHZ0
;.IF YELARM
; REFVT3-TACHZ0
; REFVT4-TACHZ0
;.ENDC
.BLKW 37. ;LIST OF SERVOS WITH TACHOMETERS
SRFADL: REFCN1 ;REFERENCE VOLTAGE SUPPLY A/D CHANNELS
REFCN2
;.IF YELARM
; REFCN3
; REFCN4
;.ENDC
.BLKW 37. ;A/D CHANNEL LIST
SRFVAL: .BLKW 38. ;A/D READINGS
CODE
;END OF "SETREF"
; STHLV6 & SRFLV6 - level 6 code for "SETTH" AND "SETREF"
;STHLV6 IS USED BY "SETTH" TO READ THE ADC POT CHANNELS. THIS ROUTINE IS
;REPEATED UNTIL ALL POTS ARE READ WITHIN WIPER RANGE OR UNTIL "STTCNT" IS
;DECREMENTED TO ZERO. "SETTH" MUST BE SCHEDULED TO RUN AT LEVEL 6.
STHLV6:
MOV #STTSRV,DAT ;SET POINTER TO SERVOS WITH POTS
MOV #42,DR11S ;SET YELLOW INTERFACE TO READ MODE
CLR OUTRGE ;ASSUME ALL WIPERS IN RANGE
MOV DAT,BC ;SAVE POINTER TO SERVOS WITH POTS
MOV (BC)+,DAT ;SET POINTER TO FIRST SERVO DATA BLOCK
MOV DAT,CC
CMP DAT,#SRV17 ;PUMA?
BHIS 10$ ;Yes - skip ahead
MOVB POTCHN(DAT),ADCCHN ;START ADC WORKING ON FIRST CHANNEL
MOVB POTCHN(DAT),DR11O ;IN BOTH INTERFACES
1$: bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 4$
TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,POT(DAT) ;SAVE ADC POT READING
BR 5$
4$: TSTB DR11S
BPL .-4 ;WAIT
MOV DR11I,POT(DAT) ;SAVE ADC POT READING
5$: MOV (BC)+,CC ;GET POINTER TO NEXT SERVO DATA BLOCK
BNE 10$ ;SKIP IF END OF LIST
CMP DAT,#SRV17 ;Was last one a PUMA?
BLO 7$ ;No - call wiper
BR 11$
10$: CMP CC,#SRV17 ;PUMA?
BLO 8$ ;No - skip ahead
CMP STTCNT,#4 ;Only need to read PUMA joints once
BNE 5$
mov cc,dat ;Set up DAT to point to data block for the PUMA
mov angadr(dat),r0 ;Set R0 to point to place to store angles.
GOSUB PANGLE ;Go read 'em
BR 5$ ;Go on to next servo
8$: bit #BLUARM+BLUHND,MECHSM(CC)
BEQ 6$
MOVB POTCHN(CC),ADCCHN ;START CONVERTING NEXT CHANNEL
BR 7$ ;*K1
6$: MOVB POTCHN(CC),DR11O
7$: JSR PC,WIPER ;CHECK IF WIPER WITHIN OPERATING RANGE
9$: MOV CC,DAT ;REPEAT TILL ALL SERVOS DONE
BNE 1$
11$: DEC STTCNT ;HAS THIS CODE RUN 4 TIMES ALREADY?
BLE 2$ ;BRANCH IF NO MORE CHANCES TO GET WIPERS IN RANGE
TST OUTRGE ;ELSE TEST IF ANY WIPERS OUT OF RANGE
BEQ 2$ ;WE ARE DONE IF NO WIPERS OUT OF RANGE
RTS PC ;DONT SIGNAL EVENT YET, DONT CLEAR LVL 6 SER. REQ.
2$: MOV OUTRGE,STTWIP ;SAVE WIPER ERROR CODE
EVSIG STTEVT ;SIGNAL END OF LEVEL 6 CODE
CLR GOSTH ;CLEAR REQ. FOR LVL 6 SERVICE
RTS PC ;RETURN
;SRFLV6 IS CALLED BY "SETREF" TO READ THE ADC TACHOMETER CHANNELS AND THE
;REFERENCE POWER SUPPLY VOLTAGE. THIS ROUTINE RESCHEDULES ITSELF TO AVERAGE
;THE READINGS "SRFCNT" NUMBER OF TIMES.
SRFLV6:
MOV #SRFSRV,DAT ;SET POINTER TO SERVOS WITH TACHOMETERS
MOV #SRFADL,DC
MOV #SRFVAL,EC ;SET POINTER TO A/D READINGS
MOV DAT,CC
1$: MOV (CC)+,DAT ;SET POINTER TO SERVO BLOCK DATA
BEQ 2$ ;BRANCH IF END OF LIST
MOV (DC)+,AC
cmp cc,#srfsrv+6 ;are we checking blue ref channels? *K
blt 6$ ;br if yes
bit #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE FOR THIS SERVO
BEQ 4$
6$: MOVB AC,ADCCHN ;BLUE INTERFACE
TSTB ADCSR
BPL .-4 ;WAIT
MOV ADCVAL,AC ;GET TACH READING
BR 5$
4$: MOV #42,DR11S ;YELLOW INTERFACE
MOVB AC,DR11O
TSTB DR11S
BPL .-4 ;WAIT
MOV DR11I,AC ;GET TACH READING
SUB #3777,AC
5$: SUB TACHZ0(DAT),AC
ASR AC ;GET A 1/2 OF THE DIFFERENCE
ADD AC,TACHZ0(DAT) ;USE THIS TO CORRECT THE OLD READING
BR 1$ ;DO ALL SERVOS WITH TACHS
2$: DEC SRFCNT ;HAVE WE FINISHED AVERAGING YET?
BLE 3$ ;BRANCH IF WE ARE ALL THROUGH
RTS PC ;DONT SIGNAL EVENT, DONT CLEAR LVL 6 SER. REQ.
3$:
EVSIG SRFEVT ;SIGNAL END OF LEVEL 6 CODE
CLR GOSRF ;CLEAR LVL 6 SER. REQUEST
RTS PC ;RETURN
;RUNSRV - schedules the servos of a device to run and waits for their completion
;THE FOLLOWING FUNCTIONS ARE PERFORMED BY THIS ROUTINE:
;
; 1) THE HARDWARE ERROR RESET IS TOGGLED AND A CHECK IS MADE TO
; ENSURE THAT THE POWER SUPPLY IS ON. IF IT'S NOT, AN ERROR FLAG
; IS RETURNED IN R0 OTHERWISE R0 IS CLEARED.
; 2) THE SERVOS LISTED IN THE DEVICE BLOCK ARE SCHEDULED FOR EXECU-
; TION AT LEVEL 6 ALONG WITH A DAC JOB TO REFRESH THE DACS.
; 3) THE TIME THAT the SERVOs are SCHEDULED TO FIRST RUN IS SAVED IN
; "PTIME" WITHIN EACH SERVO DATA BLOCK.
; 4) THE RUN BIT IS SET & srvkil bit is cleared IN EACH SERVO STATUS WORD.
; 5) IF EITHER FORCE SENSING OR COMPLIANCE IS TO BE PERFORMED WITH
; THE ARM SPECIFIED BY THE SERVOS, THE FORCE SENSING LEVEL 6&5
; JOBS ARE enabled AND SYNCHRONIZED TO THE ARM JOINTS. AN ERROR
; MESSAGE IS RETURNED AND THE FORCE SENSING IS ABORTED IF NOT
; ALL 6 OF THE ARM JOINTS ARE SCHEDULED TO RUN.
; 6) AFTER ALL SERVOS ARE COMPLETED, THIS ROUTINE KILLS THE DAC
; REFRESH JOB AND THE FORCE SENSING JOBS AND RETURNS TO THE
; CALLING ROUTINE.
;
;THIS SUBR IS CALLED WITH A PTR TO THE COEFFICIENT DATA LIST IN R0 AND
;A PTR TO THE DEVICE BLOCK IN R1
;REGISTERS USED:
; R1,R0 PASS ARGUMENT AND R0 IS ALTERED
RUNSRV: mov R5,-(sp)
mov r4,-(sp)
MOV R3,-(SP)
MOV R2,-(SP)
MOV (R1),-(SP) ;SAVE NUMBER OF SERVOS ATTACHED
; ("SERVO" DESTROYS IT OTHERWISE)
MOV R1,-(SP) ;SAVE DEVICE BLOCK POINTER
MOV (R0),R3 ;GET SERVO BITS
; MOV #C66,BRAKEP ;POINT TO BRAKE DATA LIST *D
; CLR C66
; CLR C66+2
; CLR C66+4
; CLR C66+6
; CLR C66+10
; CLR C66+12
.IFNZ TIMER
MOV #TIMES+2,TIMES ;RESET POINTER TO SAVE EXECUTION TIME DATA
.ENDC
.IFNZ FRCDAT
MOV #FDATA+2,FDATA ;RESET PTR TO FORCE DATA ARRAY
.ENDC
tst gdata ;are we gathering?
bne 1$ ;YES
tst frcnum ;any jobs waiting on force sensing?
bne 1$ ;br if sensing required *k
tst stfdat ;stiffness servoing?
beq blinit ;br if not
1$: BIT #BARM,R3 ;BARM BEING USED?
BEQ blinit ;NO
MOV #BARM,R4 ;YES, ASSUME BLUE ARM
MOV #SRV13,R2
BIT #BLUARM,@FCARAY ;WERE WE RIGHT?
BNE .+12
MOV #YARM,R4 ;NO
MOV #SRV06,R2
BIC R3,R4 ;MAKE SURE ALL 6 JTS ASSIGNED
BEQ 2$ ;OK
MOV #FWRGJT,R0 ;SIGNAL ERROR
JMP RUNDNE
2$: MOV INUSE(R2),FINUSE
bit #run,stfdat ;stiffness servoing?
beq blinit ;br if not
bit #barm,r3 ;barm?
beq blinit ;br if not
CLR KPTIME ;ZERO TICKS INTO SEGMENT
MOV DATST+SEGTME(R0),R2 ;GET FIRST segment SEGTIME
ASH #-4,R2 ;CONVERT TO SERVO PERIODS
LDCIF R2,AC0
STF AC0,KFTTIM ;STORE FP SEGTIM/16
CLR R0 ;indicate no errors
blinit: CLR BPANIC ;initialize & CLEAR BLUE ARM HARDWARE ERROR FLAG
BIT #BARM+BHAND,R3 ;BLUE ARM or blue hand?
BEQ ylinit ;no, see if it is the yellow or puma interfaces
INCB KICDAC ;NEED TO SCHEDULE DAC REFRESH JOB?
BIT #KICRUN,KICDAC
BNE bludev ;NO, STILL RUNNING
MOVB #10,174004 ;RESET BLUE ARM INTERFACE
MOV #NOPOWR,R0
BIT #6,174004 ;MOTOR POWER ON?
beq 1$ ;yes, schedule it to run
jmp decdac
1$: SCHEDU #MUGDAC,#DACSER,#USRDM,#20
BIS #KICRUN,KICDAC
bludev: bit #barm,r3 ;Is the device the blue arm?
beq 1$ ;No, it must be a blue hand
mov #barmdv,r2 ;address in device status array to put dev blk pntr
br sched
1$: mov #bhandv,r2 ;address in device status array to put dev blk pntr
br sched
ylinit: bit #yarm+yhand+screw+vise,r3 ;yellow interface being used?
beq pminit ;go initialize puma arms
MOV #3,DR11S ;CLEAR REQUEST B (YELLOW PANIC INTERUPT REQ)
TST DR11I
MOV #2,DR11S ;READ MOTOR POWER
MOV #31.,DR11O
MOV #NOPOWR,R0
TSTB DR11S
BPL .-4
BIT #3000,DR11I
BEQ RUNDNE
ylwdev: bit #yarm,r3 ;is the device the yellow arm?
beq 1$ ;No, it must be some other yellow device
mov #yarmdv,r2 ;address in device status array to put dev blk pntr
br sched
1$: bit #yhand,r3 ;is the device a yellow hand?
beq 2$ ;No, it must be some other yellow device
mov #yhandv,r2 ;address in device status array to put dev blk pntr
br sched
2$: bit #screw,r3 ;is the device a screw driver?
beq 3$ ;No, it must be the vise (the only one left)
mov #scrwdv,r2 ;address in device status array to put dev blk pntr
br sched
3$: mov #visedv,r2 ;only vise left; address in device status array to
br sched ; put dev blk pntr
pminit: ;add initialization to puma here
;*** FIX: CHECK FOR ARM CALIBRATED ***
PUSH <R0,R1>
MOV #IOREG,R1 ;Now see if arm power is on - read status word
MOV #SRV17,DAT ;Assume green arm
BIT 2(R0),#GHAND+GARM ;Is it really the green arm?
BNE 5$ ;Yes - continue
MOV #SRV19,DAT ;No - set up for red arm.
5$: GOSUB RSINGL ;Put status word into R0
MOV R0,R3 ;Copy it
POP <R1,R0>
BIT #ISON,R3 ;Is "power on" bit set?
BNE pmdev ;If so, continue
MOV #NOPOWR,R0 ;Else complain
BR RUNDNE
pmdev: bit #garm,2(r0) ;is the device the green arm?
beq 1$ ;No - keep checking
mov #garmdv,r2 ;address in device status array to put dev blk pntr
br sched
1$: bit #ghand,2(r0) ;is it the green hand?
beq 3$ ;no - keep looking
mov #ghandv,r2 ;address in device status array to put dev blk pntr
br sched
3$: bit #rarm,2(r0) ;red arm?
beq 5$
mov #rarmdv,r2 ;address in device status array to put dev blk pntr
br sched
5$: mov #rhandv,r2 ;only choice left - red hand
sched: MOV R1,R4 ;Save address of servo data block
MOVB 2(SP),R0 ;# OF SERVOS SCHEDULED
EVMAK ;THIS EVENT SIGNALS SERVOS DONE
TST (R1)+
MOV (SP),(R1)+ ;SAVE EVENT NUMBER IN DEVICE BLOCK
; copy the same ptime into all servo data blocks
initsv: mov (r1)+,dat ;get the pointer to servo data block
mov #16.,ptime(dat) ;assume servoes scheduled in 16 ms.
bis #run,(dat) ;set servo run bit
bic #srvkil,(dat) ;clear servo killed bit
SOB R0,INITSV
tst frcnum ;force sensing REQUESTED?
bne 3$ ;yep
tst stfdat ;stiffness system active?
beq 1$ ;yep
3$: bit #barm,r3 ;don't do force sensing except for barm
beq 1$
MOV #RUN+FIRST,FSTAT;enable force sensing to be run, indicate
1$: MOV r4,(r2) ;place device block pointer in device status array
EVWAIT (SP) ;WAIT FOR SERVOS TO FINISH
EVKIL ;DELETE EVENT NUMBER
TST BOVLCK ;CHECK FOR BOVER ON BLUE ARM
BEQ 2$
MOV #BOVERR,R0 ;INDICATE BACKGROUND JOB ERROR
2$: TST R3 ;A PUMA?
BEQ RUNDNE ;Yes - all done
BIT #YARM+YHAND+SCREW+VISE,R3 ;Yellow interface?
BEQ DECDAC ;BR IF NOT
MOV #1,DR11S ;ELSE DISABLE YELLOW PANIC INTERUPT ENB B
CLR DR11O
SLEEP #600. ;fix for bad panic hardware
BR RUNDNE
DECDAC: DECB KICDAC ;STOP DAC REFRESH JOB AND DISABLE BLUE PANIC INTERUPT
RUNDNE: MOV (SP)+,R1 ;RESTORE REGISTERS USED.
MOV (SP)+,(R1)
MOV (SP)+,R2
MOV (SP)+,R3
mov (sp)+,r4
mov (sp)+,r5
RTS PC
;LOCAL STORAGE AREA
DATA
dvstat:
barmdv: .word 0 ;The following entries forms a device status array.
bhandv: .word 0 ;When a device is to be servoed, a pointer to the
yarmdv: .word 0 ;device block must be placed in the appropriate entry.
yhandv: .word 0 ;An empty slot tells the servo routine that the device
scrwdv: .word 0 ;is not active and should not be servoed.
visedv: .word 0 ;When done, the servo code will clear the entry.
garmdv: .word 0
ghandv: .word 0
rarmdv: .word 0
rhandv: .word 0
.word -1 ;A -1 marks the end of the array.
CODE
;END OF "RUNSRV"
;DACSER - blue arm DAC refresh level 4 routine and arm error trap routine
DACSER: MOV #7,R0 ;NUMBER OF DACS TO REFRESH
MOV #BSRVOS,R1 ;BLUE ARM SERVO DATA BLKS
MOVB #20,DACCSR ;RESET HARDWARE TIME-OUT
1$: MOV (R1)+,R5 ;PTR TO DATA BLK
MOV #RUN+MOVING,R2 ;NEED TO REFRESH DAC?
BIC (R5),R2
BEQ 2$ ;NO
BIT #DACDNE,DACCSR ;DAC READY FOR NEW VALUE?
BEQ .-6 ;NO
MOV DACCHN(R5),DACCSR;REFRESH A DAC
2$: SOB R0,1$
TSTB KICDAC ;NEED TO KEEP RE-FRESHING?
BEQ 3$ ;NO
SLEEP #16.
BR DACSER
3$: BIC #KICRUN,KICDAC ;DISMISS FOREVER
BIS #10,174004 ;DISABLE BLUE ARM ALARM INTERRUPTS
DISMISS
;ARM ERROR INTERRUPT ROUTINE
ARMERR: BIS #1,BPANIC ;INDICATE BLUE ARM ERROR CONDITION
KRTI ;RETURN USING EMT IN KERNEL
YERR: BIS #2,BPANIC ;INDICATE YELLOW ARM ERROR CONDITION
KRTI ;RETURN VIA KERNEL
;LOCAL STORAGE AREA
DATA
BPANIC: 0 ;SET TO 1 IF BLUE, 2 IF YELLOW PANIC BUTTON HIT
KICDAC: .WORD 0 ;DAC REFRESH STATUS BITS CONTAINS:
KICRUN==100000 ;RE-FRESH JOB RUNNING
; 377 ;≠0 MEANS DONT STOP RUNNING
YEMSG: .ASCIZ /YELLOW PANIC BUTTON PUSHED!/
MUGDAC: PDBLK 4,20
CODE
;END OF DAC REFRESH ROUTINE
;PUMA - Arm driver routine for PUMA manipulator
PUMA: PUSH R1
PUSH R2
PUSH R3
PUSH R4
mov @cursrv,dat ;dat points to puma data block being serviced
bit #final,(dat) ;CHECK IF WE HAVE FINISHED MOVING THE PUMA
bne 1$
tst @inuse(dat) ;check if someone else signaled a catas. error
bpl 2$ ;branch if no error signaled
1$: bic #run,(dat)
decb @inuse(dat) ;decrement the number of active servos
br prturn ;return to "servo" routine
2$: bit #moving,(dat) ;check if puma is in motion
bne svpuma ;if in motion, go servo the puma
bis #moving+first,(dat);this is the first movement of the puma
svpuma: GOSUB evpuma ;obtain new joint angles to servo to
; tst r1 ;any error?
; beq 4$
; bis #NOPOLY,r1 ;set no puma polynomial msg
; ???? ????? ;but for now this can never happen so who cares?
; br prturn ;May want to deal with this for electric hand.
4$: ldcif schtim,ac2 ;update ptime for next run by adding
add schtim,ptime(dat) ;the time that this code will be run again
MOV ANGADR(DAT),R0 ;Move to angles in correct location
JSR PC,MOVPUM ;Move the arm now.
TST R1 ;Any error during move?
BEQ PRTURN ; if not, just exit.
MOV R1,R0
BIC #177400,R0 ;Get bad joint(s) bits
MOV R0,BADPJT(DAT) ;Leave it where RELSRV can get it
BIC #377,R1
BIS R1,(DAT) ;Set error message bit in status word
BIS #100000,@INUSE(DAT) ;Indicate fatal error
prturn: POP R4
POP R3
POP R2
POP R1
RTS PC ;puma returns to "servo"
; EVPUMA - Evaluates the new position that the PUMA must servo to
;The main difference between this and EVAL is that EVPUMA loops around for
;6 times to figure out all 6 joint values for the puma at once.
;
;This routine assumes that DAT contains a pointer to the appropriate
;PUMA data bdock. Any errors are returned in r1. r2-4 is garbaged.
EVPUMA: MOV DATPT(DAT),r2 ;GET POINTER TO START OF POLY. DATA LIST
BNE 1$
MOV #1,r1 ;indicate no polynomial error
jmp pevrtn
;ON POLYNOMIAL, CHECK IF END OF SEGMENT OR START OF FIRST SEGMENT
1$: MOV TTIME(DAT),r0 ;GET PRESENT TOTAL SEGMENT TIME
BIT #FIRST,(DAT) ;ARE WE JUST STARTING A MOTION?
BNE pfirst ;GO SET FIRST SEGMENT TIME
CMP PTIME(DAT),r0 ;CHECK IF PAST END OF SEGMENT
BLT peval ;BRANCH IF STILL IN SAME SEGMENT
;SWITCHING TO A NEW SEGMENT SAVE DYNAMIC COEF. AND CHECK IF FINAL SEGMENT
pckfin: MOV RNCODE(r2),r1 ;CHECK IF POSSIBLE EVENT TO SIGNAL
BEQ 2$ ;ZERO IF NONE OR ALREADY SIGNALED
PUSH <R0,R2,R3,R4,DAT>
EVSIG r1
POP <DAT,R4,R3,R2,R0>
CLR RNCODE(r2) ;ZERO TO INDICATE SIGNALED
2$: MOV (r2),r1 ;SAVE INCREMENT TO NEXT SEGMENT
ADD r1,r2 ;GET THE POINTER TO THE START OF THE NEXT SEGMENT
TST (r2) ;CHECK IF FINAL SEGMENT
BNE pupdat ;BRANCH IF NOT
;FINISHED MOTION, COMPUTE FINAL POSITION AND DYNAMIC COEF.
mov #6,r4 ;compute final position for all 6 joints
mov #ppoly1,r3 ;set pointer to poly array initially at joint 1
add dat,r3
mov #pth,r1 ;set pointer initially to joint angle 1
add dat,r1
pfloop: MOV (r3)+,r2 ;GET POINTER TO POLYNOMIAL LIST
LDF (r2),AC0 ;COMPUTE FINAL SET POINT FROM POLY, GET A5
ADDF -(r2),AC0 ; + A4
ADDF -(r2),AC0 ; + A3
ADDF -(r2),AC0 ; + A2
ADDF -(r2),AC0 ; + A1
ADDF -(r2),AC0 ; + A0
stf ac0,(r1)+ ;store the resulting joint angle away
sob r4,pfloop
CLR DATPT(DAT) ;INDICATE NO MORE POLYNOMIALS
BIS #final,(DAT) ;indicate that this is the last time
CLR R1 ;Indicate no errors
jmp pevrtn ;return to main routine
;NOT THE FINAL SEGMENT, UPDATE POINTERS, DYNAMIC COEF, AND SEG. TIME
pupdat: MOV r2,DATPT(DAT) ;SAVE NEW SEGMENT POINTER
MOV #6,R3 ;Update all 6 polynomials
MOV #PPOLY1,R4
ADD DAT,R4
1$: ADD r1,(R4)+
SOB R3,1$
SUB r0,PTIME(DAT) ;GET TIME INTO NEXT SEGMENT
pfirst: MOV SEGTME(r2),r0 ;SET THE TOTAL TIME OF THE NEXT SEG
BIC #FIRST,(DAT) ;PAST FIRST SEG
LDCIF r0,AC0 ;FLOAT THE TOTAL SEGMENT TIME
CMP PTIME(DAT),r0 ;CHECK IF PAST THIS NEW SEGMENT
BGE pckfin
MOV r0,TTIME(DAT) ;SAVE THE NEW TOTAL SEGMENT TIME
STF AC0,FTTIME(DAT) ;SAVE THE F.P. TOTAL TIME
;EVALUATE THE POSITION POLYNOMIAL FOR THE NEXT SERVICING PERIOD.
peval: LDCIF PTIME(DAT),AC1 ;CONVERT THE PRESENT TIME TO FLOATING POINT
mov #6,r4 ;compute desired position for all 6 joints
mov #ppoly1,r3 ;set pointer to poly array initially at joint 1
add dat,r3
mov #pth,r1 ;set pointer initially to joint angle 1
add dat,r1
DIVF FTTIME(DAT),AC1 ;GET THE FRACTIONAL TIME INTO THIS SEGMENT
peloop: MOV (r3)+,r2 ;GET POINTER TO POLYNOMIAL LIST
LDF (r2),AC0 ;EVALUATE THE POSITION POLYNOMIAL, GET A5
MULF AC1,AC0 ; x T
ADDF -(r2),AC0 ; + A4
MULF AC1,AC0 ; x T
ADDF -(r2),AC0 ; + A3
MULF AC1,AC0 ; x T
ADDF -(r2),AC0 ; + A2
MULF AC1,AC0 ; x T
ADDF -(r2),AC0 ; + A1
MULF AC1,AC0 ; x T
ADDF -(r2),AC0 ; + A0, NOW HAVE POLYNOMIAL JOINT ANGLE
stf ac0,(r1)+ ;store the resulting joint angle away
sob r4,peloop ;go back to puma eval loop
CLR R1 ;Indicate no errors
pevrtn: RTS PC ;RETURN
; When it exits, shouldn't R1 indicate which joint was in error?
; If there is an error, that is.
;SHould this be FIXed?
; MOVPUM: Moves the PUMA.
; Enter this routine with:
; R0 - Pointing to joint angles to move to.
; DAT- Pointing to the device's data block. DAT is not changed.
; Exits with R1 zeroed if no error, else R1 contains an error message indicator.
MOVPUM: GOSUB CHKANG ;See if desired angles are in range
TST R1 ;R1 is nonzero if some weren't
BEQ 5$ ;If all OK, skip
BIS #PASLIM,R1 ;Set "Out of range" msg
BR 100$ ; and exit
5$: PUSHF AC0
PUSH R0 ;Save addr of angles for later.
MOV DAT,R1 ;Set R1 to place to put results: a 6-word vector
ADD #PWORK6,R1 ;Place to put results.
MOV R0,R2 ;Place to get angles from
MOV #NJTS,R3 ;R3 counts number of joints
CLR R4 ;Use R4 for offset into tables.
10$: GOSUB TOCODE ;Convert @(R2)+ to encoder & store in (R1)+. Bump R4
SOB R3,10$
MOV DAT,R0 ;Set R0 to address of encoder counts to move to
ADD #PWORK6,R0
MOV #POSMDE+SEQMDE,R1 ;Command to send
GOSUB WVECT ;Write 5 encoder counts
; See if arm really got to the desired position.
CLR R1 ;Assume no error
POP R3 ;Get ptr to angles (from beginning of routine)
TST CTIME(DAT) ;Is the arm being calibrated? If so, don't chk.
BGT 50$ ;and exit with error flag cleared.
MOV DAT,R0 ;Set R0 to PUMA data block.
ADD #JANGLE,R0 ;Read PUMA angles, put in JANGLE
GOSUB PANGLE
CLR R0 ;Now go thru them & compare to desired pos'n
CLR -(SP) ;Top of stack marks jts which didn't go far enough.
MOV DAT,R1 ;Set R1 to data block base.
MOV #1,R2 ;Joint indicator
MOV #NJTS,R4
20$: LDF @(R3),AC0 ;Get desired angle
SUBF @JANGLE(R1),AC0 ;Subtract actual angle ==> error
ABSF AC0 ;Use absolute value of error
COMPF MAXDIF(R0),AC0 ;Is max.diff > error? If so, it's OK
BGT 30$
LDF @LASTTH(R1),AC0 ;Make sure speed is below threshold. Get last angle
SUBF @JANGLE(R1),AC0 ;and subtract this angle ==> change in angle
ABSF AC0 ; use absolute value
COMPF SPDTHR(R0),AC0 ;Is Threshold < Speed?
BLT 30$ ; if so, OK - continue
BIS R2,(SP) ;Set bit for "EXCESSIVE FORCE" message (on stack)
30$: LDF @JANGLE(R1),AC0 ;Now copy current angles into LASTTH for next time
STF AC0,@LASTTH(R1)
CMP (R0)+,(R0)+ ;Bump pointers.
TST (R1)+
TST (R3)+
ASL R2
SOB R4,20$
MOV (SP)+,R1 ;Get joint error indicator off stack.
TST R1 ;Did any joints not go far enough?
BEQ 50$ ;If all did, br ==> OK
MOV R1,R3 ;Save R1 for error message
MOV #STOPJT,R0 ;Set up "Stop joint motion" command
MOV #STOPCM+SEQMDE,R1 ; so the arm won't move if someone
JSR PC,WVECT ; turns it back on again.
MOV R3,R1 ;Get error message bits.
BIS #EXJTFC,R1 ;Set bits for EXCESSIVE FORCE message
50$: POPF AC0
100$: RTS PC
; PANGLE - reads current PUMA joint angles
; Routine to read current joint angles.
; Encoder counts are read from the encoders and converted into degrees.
; If the encoder read takes too long to execute, a fatal error is signalled and
; the program is halted.
; Enter this routine with R0 pointing to the place to save the angles and
; DAT pointing to the data block for the arm. No registers are changed.
PANGLE: PUSH R0 ;Save registers
PUSH R1
PUSH R2
PUSH R3
PUSH R4
MOV R0,R3 ;Save destination address for later.
SUB #<2*NJTS>,SP ;Make room on stack for encoder counts
MOV SP,R0 ;Set starting address of destination.
MOV #RDPOS+SEQMDE,R1 ;Command = read position (encoders)
GOSUB RVECT ;Read encoder counts & store on stack
CLR R1 ;Offset into conversion factor tables
MOV #NJTS,R4 ;
5$: MOV (R0)+,R2 ;Get next encoder count ==> R2
GOSUB TOANG ;Convert to an angle, store in @(R3)+
CMP (R1)+,(R1)+ ;Bump offset into tables.
SOB R4,5$
ADD #<2*NJTS>,SP ;Restore stack pointer
POP R4
POP R3
POP R2
POP R1
POP R0
RTS PC
; TOANG, TOCODE - converts to/from PUMA encoder to degrees
; TOANG: Converts encoder reading to degrees.
; Register setup:
; R1 = Word index for selected joint (0→N*4)
; R2 = Encoder reading
; R3 = Vector to store result in. R3 is incremented.
; DAT= Address of data block for this arm.
TOANG: PUSHF AC0
SUB #100000,R2 ;Make it signed.
LDCIF R2,AC0 ;Put in floating-pt register.
DIVF ESCALE(R1),AC0 ;Convert to degrees
ADDF EOFFST(R1),AC0 ;And add offset
; *** Fix this part -- joint 4,5,6 coupling factors needed ***
CMP R1,#<NJTS*4-4> ;Is it joint 5? (coupled joint?)
BNE 5$ ;If not, exit
PUSHF AC1 ;Yes - save AC1
LDF @-2(R3),AC1 ;Put angle4 in AC1
MULF CFACTR,AC1 ;and multiply by coupling factor
SUBF AC1,AC0 ;subtract theta4*cfact from theta5.
POPF AC1 ; (restore AC1)
5$: STF AC0,@(R3)+ ;Store angle in vector and bump ptr.
POPF AC0 ;Restore AC0
RTS PC
; TOCODE: Converts angles in degrees to encoder counts.
; Register setup:
; R1 = Vector to store result in. R1 is incremented.
; R2 = Vector to get angle from. R2 is incremented.
; R4 = Word index for selected joint (0→N*4). R4 is incremented.
; DAT= Address of data block for this arm.
TOCODE: PUSHF AC0
LDF @(R2)+,AC0 ;Get the angle
CMP R4,#<NJTS*4-4> ;Is it a coupled joint? (joint 6?)
BNE 5$ ;If not, skip
PUSHF AC1 ;Yes - save AC1
LDF @-4(R2),AC1 ;Get angle 4
MULF CFACTR,AC1 ;And multiply by coupling factor
ADDF AC1,AC0 ;add theta4*cfact to theta5.
POPF AC1
5$: SUBF EOFFST(R4),AC0 ;Convert to a code by subtracting offset
MULF ESCALE(R4),AC0 ;then multiplying by scale factor
STCFI AC0,(R1) ;Store encoder count
ADD #100000,(R1)+ ; and add 100000 to make it unsigned.
CMP (R4)+,(R4)+ ;Bump to next place.
POPF AC0
RTS PC
; CHKANG - checks to make sure within joint range
; CHKANG - Checks angles the arm is supposed to move to to see if they
; are all in range. The six joint angles are pointed to by R0.
; If a joint is past its limit, we set the joint angle TO its limit and
; set R1 to indicate which joint(s) are out of range.
CHKANG: PUSH R4
PUSH R2
PUSH R0
PUSHF AC0
CLR R2 ;R2 is joint # (offset)
CLR -(SP) ;Use this for which joints are bad
MOV #1,R1 ;Joint number indicator in case out of range.
MOV #NJTS,R4 ;R4 counts number of joints
5$: LDF @(R0),AC0 ;Get joint angle
COMPF LOSTOP(R2),AC0 ;Is LowStop > Angle? If so, it's out of bounds
BGT 50$ ; (so go set to limit)
COMPF HISTOP(R2),AC0 ;Is HighStop < Angle? If so, out of bounds
BLT 60$ ; (so go set to limit)
10$: CMP (R2)+,(R2)+ ;Bump offset pointer
TST (R0)+
ASL R1
SOB R4,5$
MOV (SP)+,R1 ;Get error indicator, 0=none
POPF AC0
POP R0 ;Restore R0 to what it was upon entry!!
POP R2
POP R4
RTS PC
50$: MOV LOSTOP(R2),@(R0) ;Move low limit to this angle
MOV LOSTOP+2(R2),@2(R0) ; (takes two moves because it's
BR 70$ ; floating-point)
60$: MOV HISTOP(R2),@(R0) ;Move high stop to this angle
MOV HISTOP+2(R2),@2(R0)
70$: BIS R1,(SP) ;Set error flag for return
BR 10$
; WVECT - writes a vector to the PUMA microprocessors
; WVECT: Called with
; R0=address of 5 data words to send
; R1=command.
; DAT=address of device data block.
; No registers are changed.
WVECT: PUSH R0
PUSH R2 ;a routine to write 5 words to the PUMA.
PUSH R3
PUSH R4
;DEBUGGING!!
CMP R1,#230 ;Is this an encoder change?
BNE OK230 ;No - continue
TST CHK230 ;Want to check for encoder changes?
BEQ OK230 ;No - continue
BPT ;HAH - encoder change!
BR OK230
DATA
CHK230: .WORD 0 ;CHANGE TO 1 FROM DDT.
CODE
OK230:
;END DEBUG.
MOV #NJTS,R2
MOV DRADDR(DAT),R3 ;Set R3 to DR11C status register addr
MOV #1,SRVERR(DAT) ;reset joint number counter.
CLR (R3)+ ;the familiar sequence: CSR←0
MOV R1,(R3) ;command to OBUF
INC -2(R3) ;CSR←1
10$: MOV (R0)+,(R3) ;move a word to output
MOV #40.,R4 ;Wait 40 cycles for ready bit
15$: TSTB @DRADDR(DAT) ;wait for status flag to come on ('200 bit)
BMI 20$ ;if ready, exit
SOB R4,15$ ;keep trying
MOV SRVERR(DAT),R1 ;which joint is dead
BIS #SRVDEA,R1 ;set higher bits.
JMP SEPUKU ;report fatal error and die
20$: INC SRVERR(DAT) ;Add to joint number
SOB R2,10$ ;do all the joints
POP R4
POP R3
POP R2
POP R0
RTS PC
; RVECT - reads a vector from the PUMA microprocessor
; RVECT: Called with
; R0=address of vector to put 6 read-in data words
; R1=command.
; DAT=address of device data block.
; No registers are changed.
RVECT: PUSH R0
PUSH R2
PUSH R3
PUSH R4
MOV #NJTS,R2
MOV DRADDR(DAT),R3 ;Set up DR11C status address.
MOV #1,SRVERR(DAT) ;reset joint number counter.
CLR (R3)+ ;the familiar sequence: CSR←0
MOV R1,(R3)+ ;command to OBUF
INC -4(R3) ;CSR←1
10$: MOV #100.,R4 ;We'll wait this long for joint to be ready.
15$: TSTB -4(R3) ;Is it ready yet?
BMI 50$ ;If so, exit & read data from it
SOB R4,15$ ;No, keep checking
; Joint did not respond. Figure out if it was an ADC or encoder read.
CMP R1,#READC+SEQMDE ;ADC read?
BNE 20$ ;No
MOV #ADCDEA,R1 ;Say ADC dead
BR 30$
20$: MOV #SRVDEA,R1 ;Say servo dead = encoder not working
30$: BIS SRVERR(DAT),R1 ;And set in error indicator
JMP SEPUKU
50$: MOV (R3),(R0) ;Get an input word
CMP R1,#READC+SEQMDE ;Is this an ADC read?
BNE 80$ ;If not, skip
BIC #177400,(R0) ;Only look at low 8 bits from ADC.
80$: TST (R0)+
INC SRVERR(DAT) ;Add to joint counter
SOB R2,10$ ;do all the joints
POP R4
POP R3
POP R2
POP R0
RTS PC
; WSINGL, RSINGL - read and write single word to/from PUMA micro's
; WSINGL: Called with R0=data to send, R1=command, DAT=data block addr
WSINGL: PUSH R3
MOV DRADDR(DAT),R3 ;Set R3 to DRSTAT for this arm.
CLR (R3)+ ;the familiar sequence: CSR←0
MOV R1,(R3) ;command to OBUF
INC -2(R3) ;CSR←1
10$: MOV R0,(R3) ;move a word to output to the micro
POP R3
RTS PC
; RSINGL: Called with R1=command, DAT=data block addr.
; Exits with R0=data from micro.
RSINGL: PUSH R3
MOV DRADDR(DAT),R3 ;R3 to DRSTAT.
CLR (R3)+ ;the familiar sequence: CSR←0
MOV R1,(R3)+ ;command to OBUF
INC -4(R3) ;CSR←1
10$: MOV (R3),R0 ;get input from micro
POP R3
RTS PC
; BITON & BITOFF routines
; BITON and BITOFF use the bits set in R0 to set/clear bits in
; ARMBIT as desired. The resulting value of ARMBIT is then sent
; to the microprocessor interface board. E.g. to set bits 1 and 2,
; set R0 to 007 and call BITON; to clear them, call BITOFF.
; Call these routines with R0 set as described as above, and DAT
; pointing to the device data block.
BITOFF: BIC R0,ARMBIT(DAT) ;clear bits in ARMBIT according to R0.
BR SETIT1
BITON: BIS R0,ARMBIT(DAT) ;set bits in ARMBIT according to R0.
SETIT1: MOV ARMBIT(DAT),R0 ;move byte to send
COMB R0 ;and complement the low-order bits (OX lines)
PUSH R1 ;Save R1
MOV #IOREG,R1 ;and command
CALL WSINGL ;Write ARMBIT to the micros
POP R1
RTS PC
; CALIB - Puma calibration routine
;This routine is called at the start of an AL program to calibrate the PUMA arm(s).
;R0 contains the mech bits of the arms to calibrate.
;SAMPLE CALLING SEQUENCE:
; MOV #mechs,R0 ;What to calibrate
; JSR PC,CALIB
CALIB: PUSH <R3,R4,DAT> ;Save registers for interpreter.
PUSH #-1 ;Use this to mark end of DAT pointers on stack
BIT #GRNARM,R0 ;Want to calibrate green arm?
BEQ 5$ ; if not, br
MOV #SRV17,DAT ; yes - set DAT to green arm's data block
PUSH DAT ;Save ptr on stack for calibration later.
JSR PC,PUMINI ;Initialize the arm so user can turn it on.
5$: BIT #REDARM,R0 ;Want to calibrate red arm?
BEQ 10$ ;If not, exit
MOV #SRV19,DAT ;Yes - calibrate red arm
PUSH DAT ;Store this ptr on the stack too.
JSR PC,PUMINI ;Just initialize the arm.
10$: MOV #CALMS0,R0 ;Tell user he can turn arms on now.
JSR PC,@LTYPSTR
MOV #IOBUF,R0 ;Wait for response
JSR PC,@LINSTR
15$: POP DAT ;Get ptr to 1st arm to calibrate.
CMP DAT,#-1 ;End of list?
BEQ 20$ ;Yes - exit
JSR PC,CALINI ;A valid pointer - calibrate the arm.
BR 15$ ;Look for more to calibrate.
20$: POP <DAT,R4,R3> ;Restore interpreter's registers
RTS PC
; Here's the heart of the calibration routine, called by CALIB above.
; DAT is set to the data block of the arm to be calibrated.
CALINI: PUSH R0 ;Save R0 for caller (see above)
; Read the encoders. If they're all near octal 400, we'll crudely initialize
; the arm.
MOV DAT,R0 ;Set R0 to data block base
ADD #PWORK6,R0 ;Put encoder counts in PWORK6.
MOV #RDPOS+SEQMDE,R1
JSR PC,RVECT
MOV DAT,R0 ;Again, set R0 to PWORK6 for this arm.
ADD #PWORK6,R0 ;Look thru and check them.
MOV #6,R1
CLR R2 ;R2 counts how many were near 400.
1$: SUB #376,(R0) ;Needs to be close to 376
BPL 2$ ;Make it positive if it's negative
NEG (R0)
2$: CMP (R0),#5 ;Is it within 5?
BGT 3$ ;If not, continue
INC R2 ;Add to # found that were about 400.
3$: TST (R0)+
SOB R1,1$
TST R2 ;Is arm already set up?
BNE 5$ ;If not, set encoders crudely.
TST CALFLG(DAT) ;Encoders are OK. Is arm calibrated?
BEQ CALPUM ;No - go see if they want to calibrate.
BR CALDNE ;Arm already calibrated. Just exit.
5$: JSR PC,SETENC ;Set encoder counts crudely...
MOV DAT,R0 ;Tell user which arm we're talking about.
ADD #PNAME,R0 ;by setting addr of name of arm in R0
JSR PC,@LTYPSTR ;and printing the name.
MOV #CALMS1,R0 ;Say "Arm crudely initialized."
JSR PC,@LTYPSTR
CALPUM: MOV #CALM2A,R0 ;ASK IF THEY WANT TO CALIBRATE
JSR PC,@LTYPSTR
MOV DAT,R0 ;TELL THEM WHICH ARM IT IS
ADD #PNAME,R0
JSR PC,@LTYPSTR
MOV #CALM2B,R0
JSR PC,@LTYPSTR
MOV #IOBUF,R0 ;GET RESPONSE, PUT IN IOBUF
JSR PC,@LINSTR
MOVB IOBUF,R0 ;PUT ANSWER IN R0
BIC #40,R0 ;MAKE IT UPPER CASE
CMPB R0,#'Y ;IF THEY WANT TO, DO IT
BEQ 4$
JMP CALDNE ;ELSE EXIT.
; Here's where calibration begins ...
4$: JSR PC,SETENC ;SET ENCODERS ROUGHLY IN CASE THEY GOT MESSED UP.
MOV #200.,CTIME(DAT) ;START TIMER
5$: JSR PC,GOCAL ;START CALIBRATING
TST R1 ;ANY ERROR IN CALIBRATION PART?
BNE 7$ ;IF SO, GO PRINT IT
TST CTIME(DAT) ;ARE WE DONE?
BMI 10$ ;IF SO, EXIT
SLEEP #32. ;WAIT FOR 32 MSEC.
BR 5$ ;KEEP MOVING ARM & LOOK FOR ZERO INDICES.
7$: PUSH R1 ;Save error bits
MOV DAT,R0 ;TELL THEM WHICH ARM
ADD #PNAME,R0
JSR PC,@LTYPSTR
MOV #CALMS4,R0 ;PRINT "CALIB DIED"
JSR PC,@LTYPSTR
POP R1
JSR PC,ERRPRN ;Print error message
BR CALDNE ;and exit
10$: MOV #ENBMDE,R0 ;RESET STATUS SO ARM WILL MOVE.
MOV #SMODE+SEQMDE,R1
JSR PC,WVECT
MOV DAT,R0
ADD #PNAME,R0
JSR PC,@LTYPSTR
MOV #CALMS3,R0 ;SAY ARM IS CALIBRATED.
JSR PC,@LTYPSTR
MOV #1,CALFLG(DAT) ;MEANS ARM IS NOW CALIBRATED.
CALDNE: POP R0 ;RESTORE R0 FOR CALLER.
RTS PC
DATA
CALMS0: .ASCII /ARM POWER FOR PUMA(S) ENABLED. TURN ARM POWER ON NOW./
.BYTE 15,12
.ASCIZ / π(TYPE <CR> TO CONTINUE).../
CALMS1: .ASCII / PUMA CRUDELY INITIALIZED.../
.BYTE 15,12,0
CALM2A: .ASCIZ /DO YOU WANT TO CALIBRATE THE /
CALM2B: .ASCIZ / PUMA? /
CALMS3: .ASCII / PUMA IS CALIBRATED./
.BYTE 15,12,0
CALMS4: .ASCIZ / PUMA: CALIBRATION ERROR! /
.EVEN
CODE
; PUMINI: Initializes the arm
;Uses no registers; the only one that needs to be set upon calling this
;routine is DAT, which must point to the data block for the arm to be
;initialized.
PUMINI: PUSH R0 ;Save registers
PUSH R1
MOV #ZICNT,R0 ;Counts btw zero indexes
MOV #SMODE+SEQMDE,R1 ;store in micro memory
CALL WVECT ;send 5 words to the micros
MOV #VELGAN,R0 ;set velocity error gain
MOV #SMODE+SEQMDE,R1
CALL WVECT
MOV #TOLRGE,R0 ;Set narrow in range tolerance limits
MOV #STOLRG+SEQMDE,R1
CALL WVECT
MOV #INTRGE,R0 ;set hardware integration region
MOV #SINTRG+SEQMDE,R1
CALL WVECT
MOV #ENBMDE,R0 ;to enable integration
MOV #SMODE+SEQMDE,R1
CALL WVECT
CLR ARMBIT(DAT) ;clear ARMBIT: reset all hardware bits
MOV #ENABLE,R0 ;Enable high power for arm
CALL BITON ;Set bit on in joint 7 status word
POP R1
POP R0
RTS PC
; GOCAL: The main part of the calibration routine
GOCAL: CMP CTIME(DAT),#200. ;first pass thru this routine?
BNE 10$
MOV #1,R3 ;1st time. Read initial pot values into INIPOT
MOV #INIPOT,R2 ;Put pot readings in INIPOT
JSR PC,RPOT ;Read joint pots
MOV ANGADR(DAT),R0 ;Also read current joint angles. Put them in here.
JSR PC,PANGLE
10$: CMP CTIME(DAT),#120. ;Time to start looking for zero index?
BNE 20$ ;if not, skip
MOV #CALMDE,R0 ;Yes - Set up joints for calibration mode
MOV #SMODE+SEQMDE,R1
JSR PC,WVECT ;send 5 words to the micros
20$: DEC CTIME(DAT) ;no zero index error?
BPL 23$ ;If not, continue
MOV #NOZIND,R1 ;Error - no zero index when expected.
ADD BADBIT,R1 ;indicate which joint.
BR CALEXI ;and exit
23$: MOV DAT,R0 ;Arm is still moving. Read statuses into PWORK6
ADD #PWORK6,R0
MOV #RDSTAT+SEQMDE,R1
JSR PC,RVECT
MOV #NJTS,R3 ;Now look thru statuses to see if all found zero ref
MOV #1,R2 ;Marks which joint we're at
CLR R4 ;mark jnts which have not found zero w/bits in R4.
25$: BIT #1000,(R0)+ ;Look at input buffer. Found zero reference?
BNE 30$ ;if so, br
BIS R2,R4 ;no zero ref yet - mark with a bit
30$: ASL R2 ;joint indicator
SOB R3,25$ ;Do all 6 joints
MOV R4,BADBIT ;Any joints still moving?
BEQ GOTZER ;No, go to next step of calibration - got zeros
MOV ANGADR(DAT),R1 ;Now add increment to each joint (move it)
MOV #NJTS,R2
MOV #CALMVE,R3 ;set to table of angles to move each jt during calib
40$: LDF (R3)+,AC0 ;Get angle increment
ADDF @(R1),AC0 ;Add current angle
STF AC0,@(R1)+ ;and store back into current angle.
SOB R2,40$
MOV ANGADR(DAT),R0 ;Set address of joint angles to move to.
JSR PC,MOVPUM ;Move arm to position in ANGADR
;MOVPUM returns error codes in R1. Just pass them back to caller.
RTS PC
GOTZER: NEG CTIME(DAT) ;Indicate we found zero references
MOV #1,R3 ;Make sure pot values changed from initial
MOV #FINPOT,R2 ;Put final readings in here
JSR PC,RPOT ;Read joint pots, store in FTH.
MOV #BADPOT,R0 ;Error code if pot didn't change
MOV #1,R3 ;Joint bit mask
MOV #NJTS,R1 ;Counter
10$: CMP INIPOT-FINPOT(R2),(R2) ;Initial - Final pot readings.
BNE 60$ ;If they're diferent, OK
BIS R3,R0 ;If equal, mark error.
60$: ASL R3 ;To next joint
TST (R2)+ ;Bump R2
SOB R1,10$
MOV R0,R1 ;Move error msg in case of error
TSTB R0 ;Any error?
BNE CALEXI ;If so, exit.
JSR PC,SETENC ;Set encoders and DANGLE based on pots
CLR R1 ;No error - clr R1 to indicate this.
CALEXI: RTS PC
; end of GOCAL
DATA
BADBIT: .WORD 0 ;BADBIT - which joint is in error, for calibration
INIPOT: .BLKW 6 ;INIPOT - Initial pot readings - for calibration
FINPOT: .BLKW 6 ;FINPOT - final " " " "
CODE
; SETENC routine
; SETENC - Sets encoder counters and GTH/RTH according to joint pot readings.
; Averages the reading from the joint pots and uses the nominal reading to
; determine the absolute value that each joint encoder should have, assuming
; that all of the joints are servoing at a zero reference point.
; DAT should be pointing to the device data block upon entry.
; Registers used: R0, R1, R2, R3, R4. Also AC0, AC1.
SETENC: MOV #128.,R3 ;Read each pot 128. times
MOV DAT,R2 ;Put results into PWORK6 in device's data block.
ADD #PWORK6,R2 ;Place to put results.
JSR PC,RPOT
MOV DAT,R0 ;Use R0 for index into tables. Set to block base.
MOV DAT,R1 ;Where to get ADC counts - from data block
ADD #PWORK6,R1 ;with an offset
MOV ANGADR(DAT),R2 ;Now set R2 to associated joint angles
MOV #NJTS,R3 ;Now convert ADC's to angles
CLR R4 ;Use R4 for an offset into EDELTA.
10$: LDCIF (R1),AC0 ;Get next ADC count * 128.
MULF ADCSCL(R0),AC0 ;Convert to degrees. This is approximate.
ADDF ADCOFS(R0),AC0
SUBF EFIRST(R0),AC0 ;Now round to nearest zero index. Subtract offset
DIVF EDELTA(R4),AC0 ; Divide by degrees/zero index ==> how many indexes
ADDF FLTPT5,AC0 ; Now round: Add 0.5
CHECKF AC0 ; But if angle is < 0, we want to SUBTRACT 0.5
BGE 12$
SUBF FLT1,AC0 ; so -1.0 + 0.5 = -0.5
12$: MODF FLT1,AC0 ; Truncate & put integer part in AC1.
MULF EDELTA(R4),AC1 ; Then multiply by delta again.
ADDF EFIRST(R0),AC1 ; Add back the offset
STF AC1,@(R2) ;Store exact angle in GTH.
JSR PC,TOCODE ;Convert @(R2)+ to encoder, store in (R1)+. Bump R4.
CMP (R0)+,(R0)+ ;Bump R0.
SOB R3,10$ ;Do all the joints
; At this point, encoders are in PWORK6 and angles are in ANGADR.
MOV DAT,R0 ;Now set the encoder counts in the micros
ADD #PWORK6,R0
MOV #SETPOS+SEQMDE,R1 ;Command to send = Set position
JSR PC,WVECT
MOV ANGADR(DAT),R0 ;Check angles which are in DANGLE
JSR PC,CHKANG ;Check joint angles - are they in range?
TST R1 ;R1 is set if some aren't
BEQ 20$ ;If all OK, br
MOV #SETMS1,R0 ;Say SETENC died
JSR PC,@LTYPSTR
BIS #PASLIM,R1 ;Set bits to mean fatal, out-of-range
JSR PC,ERRPRN ;Type message - out of range!
;May want to do something here, like free up selected joints.
20$: RTS PC
DATA
FLTPT5: .FLT2 0.5
SETMS1: .ASCIZ /SETENC - ANGLES OUT OF RANGE!/
.EVEN
CODE
; RPOT routine
; RPOT - Read joint pots.
; Call with:
; R2=place to put sum of ADC readings in
; R3=number of times to read.
; DAT=addr of data block for the arm.
RPOT: PUSH R0
PUSH R1
PUSH R4
MOV #NJTS,R0 ;clear 6 words starting at (R2)
1$: CLR (R2)+
SOB R0,1$
SUB #<2*NJTS>,R2 ;Put R2 back where it started.
SUB #<2*NJTS>,SP ;Make room on stack for ADC counts
MOV SP,R0 ;Set address where to put ADC readings from RVECT
MOV #READC+SEQMDE,R1 ;Set starting command = read ADC's
5$: JSR PC,RVECT ;Read ADCs and put on the stack
MOV #NJTS,R4 ;Now add to running total
10$: ADD (R0)+,(R2)+ ; (add this adc to total)
SOB R4,10$ ; (and do this for 6 joints)
SUB #<2*NJTS>,R2 ;Back R2 to original place
SUB #<2*NJTS>,R0 ;and back up R0 also.
SOB R3,5$ ;Now keep reading as many times as in R3.
ADD #<2*NJTS>,SP ;Restore stack pointer
POP R4
POP R1
POP R0
RTS PC
; ERRPRN: Puma error print routine
; Called with R1 containing, in its low byte, the bits for the joints
; in error, and in its high byte an offset into the error table.
; DAT should point to the data block for the PUMA in error, so we can
; get its name and print it.
ERRPRN: PUSH R0 ;Called with R1 = offset into error table
PUSH R2 ; (in high byte)
PUSH R5
PUSH R1 ;Need to save R1 because LTYPSTR uses it...
MOV DAT,R0 ;Print which PUMA it is
ADD #PNAME,R0 ;by getting the addr of its color
JSR PC,@LTYPSTR ;and typing it.
MOV #ERRNAM,R0 ;print "puma: "
JSR PC,@LTYPSTR
POP R1
;Check for special case: PASLIM isn't contiguous with the rest of the
;error codes. So if the error is PASLIM, make the error code zero.
BIT #PASLIM,R1 ;Is it PASLIM?
BEQ 5$ ; if not, continue
BIC #177400,R1 ;Yes - make error code 0 by clearing high byte.
5$: MOV R1,R5
SWAB R5 ;Put offset into error table into low byte
BIC #177741,R5 ;Just look at low 5 bits & make it even.
MOV ERRTBL(R5),R2 ;Get offset into table
MOV #IOBUF,R5 ;Move messsage text to buffer to print
10$: MOVB (R2)+,(R5)+
BNE 10$
MOVB #40,-(R5)
MOV #60,R0 ;ASCII zero - form what joint was in error.
BIT #20000,R1 ;Is decimal joint # already set up?
BEQ 20$ ;If not, then br
BISB R1,R0 ;Get bad joint # (only one of them, not a list)
CLR R1 ;Now we'll exit right away.
20$: BIC #17000,R1 ;If no bits set, exit & print
BEQ 25$
22$: INC R0
ASR R1 ;If this bit was set, print a digit for it
BCC 30$
25$: MOVB R0,(R5)+
MOVB #40,(R5)+ ;space following joint number
30$: BNE 22$
CLRB (R5)
ALERR IOBUF ;Print what's in the buffer & enter DDT
POP R5
POP R2
POP R0
RTS PC
SEPUKU: MOV #-1,R0 ;Turn off arm power.
GOSUB BITOFF
MOV #77777,R0
1$: SOB R0,1$ ;Kill some time to make sure it took effect.
GOSUB ERRPRN ;PRINT ERROR MESSAGE
5$: BPT ;go to ODT!
BR 5$ ;no way are they getting out of this one.
DATA
ERRTBL: .WORD MSG0 ;OFFSET 00 - SET MANUALLY FOR "PAST LIMIT" MSG
.WORD MSG1 ;OFFSET 02 - BAD POT
.WORD MSG2 ;OFFSET 04 - NO ZERO INDEX
.WORD MSG3 ;OFFSET 06 - ADC DEAD
.WORD MSG4 ;OFFSET 10 - EXCESSIVE JOINT FORCE (COLLISION)
.WORD MSG5 ;OFFSET 12 - SERVO DEAD.
ERRNAM: .ASCIZ / PUMA: /
MSG0: .ASCIZ /PAST STOP LIMIT --- JOINT /
MSG1: .ASCIZ /BAD POT (POT DIDN'T CHANGE) --- JOINT /
MSG2: .ASCIZ /NO ZERO INDEX --- JOINT /
MSG3: .ASCIZ /ADC DEAD (CAN'T READ IT) --- JOINT /
MSG4: .ASCIZ /EXCESSIVE JOINT FORCE (COLLISION!) --- JOINT /
MSG5: .ASCIZ /SERVO DEAD (WON'T RESPOND) --- JOINT /
.EVEN
IOBUF: .BLKW 50
CODE
PATCHX: .BLKW 50. ;Room for patching, in Code space!
;TOUCH - schedules touch events
;THIS PROGRAM IS USED FOR enabling EVENTS THAT ARE TO BE TRIGGERED ACCORDING
;TO THE STATE OF A TOUCH SENSOR. THE EVENT SIGNALING CAN TAKE PLACE WHEN A
;TOUCH SENSOR READS ON OR OFF. AS MANY AS TWENTY-FIVE DIFFERENT EVENTS CAN
;BE WAITING FOR VARIOUS TOUCH SENSOR STATES. READING OF THE TOUCH SENSORS AND
;DECIDING WHICH EVENTS ARE TO BE ACTIVITED IS DONE AT LEVEL 6. THE LEVEL 6
;JOB IS run as a subroutine call from "servo" ONLY WHEN SOME EVENTS ARE PENDING.
;THE WORDS CONTAINING THE STATUS OF EACH TOUCH SENSOR ARE UPDATED EACH TIME
;THE LEVEL 6 JOB IS RUN.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #EVENT,R0 ;EVENT TO BE TRIGGERED ON TOUCH
; MOV #SENSOR,R1 ;INDICATES TOUCH SENSOR AND STATE
; ; NEED TO TRIGGER EVENT
; JSR PC,TOUCH
;
;IF THE HIGH BYTE OF THE SENSOR NUMBER IS ZERO, THE OFF CONDITION WILL TRIGGER
;THE EVENT, ELSE THE ON CONDITION OF THE SENSOR WILL BE USED AS A TRIGGER.
;THE LOW BYTE OF THE "SENSOR" NUMBER MUST CONTAIN THE LOGICAL NUMBER FOR THE
;SENSOR TO BE TESTED. THE LOGICAL NUMBERS ASSIGNMENTS ARE AS FOLLOWS:
;
; SENSOR LOGICAL NUMBER
; BLUE HAND, RIGHT SIDE 0
; BLUE HAND, LEFT SIDE 1
; YELLOW HAND, RIGHT SIDE 2
; YELLOW HAND, LEFT SIDE 3
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR OCCURRED.
;THE ERROR CODES ARE LISTED ON PAGE 8.
;DEFINITIONS:
MAXTCH ==2 ;TOTAL NUMBER OF OPERATIONAL TOUCH SENSORS
TCHBLK ==25. ;NUMBER OF F.S. BLOCKS IN TOUCH LIST
TCHSCH ==400 ;LEVEL 6 STATUS BIT, INDICATES LEVEL 6 CODE SCHEDULED
RUNTCH ==1000 ; " " " " , ALWAYS RE-SCHEDULE LEVEL 6 CODE
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
TOUCH: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R1,R2
BIC #177400,R1 ;GET SENSOR NUMBER
CMP #MAXTCH,R1 ;MUST BE LESS THAN THIS
BGT TCHOK
TCHERR: MOV #UNKTCH,R0 ;SIGNAL ERROR
BR TCHDNE
TCHOK: ASL R1 ;WANT WORD INDEX
ADD #TCHOFF,R1 ;GET PTR TO TRIGGER ON OFF STATE
BIT #177400,R2 ;CHECK IF TRIGGER USING ON OR OFF STATE
BEQ .+6
ADD #TCHON-TCHOFF,R1 ;GET PTR TO TRIGGER ON ON STATE
;PUT TOUCH EVENT IN EVENT LIST
INCB TCHUP ;LOCK OUT LEVEL 6 CODE FROM USING EVENT LIST
MOV LSTUSE,R3 ;START CHECKING FROM LAST SLOT USED
MOV #TCHBLK,R2 ;CHECK ALL EVENT SLOTS FOR A FREE ONE
1$: SUB #4,R3 ;CHECK NEXT SLOT
BGT .+6 ;WRAP AROUND IF AT BOTTOM OF LIST
MOV #TCHBLK*4-4,R3
TST EVTLST(R3) ;CHECK IF THIS EVENT SLOT FREE
BEQ 2$ ;BRANCH IF EMPTY
SOB R2,1$ ;CHECK ALL AVAILABLE SLOTS
MOV #TOMTCH,R0 ;NO EMPTY SLOTS, SIGNAL ERROR
BR 3$
2$: MOV R3,LSTUSE ;SAVE PTR TO LAST SLOT USED
ADD #EVTLST,R3 ;GET ABSOLUTE ADDRESS OF FREE SLOT
MOV R0,(R3) ;PUT IN EVENT TO BE TRIGGERED
MOV (R1),2(R3) ;SAVE PTR TO PREVIOUS EVENT TIED TO THIS
; TOUCH SENSOR CONDITION
MOV R3,(R1) ;SAVE PTR TO THIS EVENT
INC ACTNUM ;INCREASE THE NUMBER OF SLOTS NOW TAKEN
BIS #TCHSCH,TCHUP ;indicate that level 6 code should be run
;by "servo" when its done moving arms
CLR R0 ;INDICATE NO ERRORS
3$: DECB TCHUP ;ALLOW LEVEL 6 CODE TO USE EVENT LIST
;EXIT CLEANLY
TCHDNE: MOV (SP)+,R3 ;RESTORE REGISTERS
MOV (SP)+,R2
RTS PC
;END OF "TOUCH"
; CLRTCH - takes a scheduled event off of the touch sensor event lists
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "TOUCH". A SAMPLE CALLING
;SEQUENCE TO "CLRTCH" FOLLOWS:
;
; MOV #EVENT,R0 ;EVENT TO BE TAKEN OFF LIST
; MOV #SENSOR,R1 ;INDICATES TOUCH SENSOR AND STATE
; ; WITH WHICH EVENT WAS ASSOCIATED
; JSR PC,CLRTCH
;
;THE INTERPRETATION OF THE "EVENT" AND "SENSOR" NUMBERS IS THE SAME AS IN
;"TOUCH". AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO
;ERROR OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8. IF NO TOUCH EVENT
;WAS FOUND, THE NORMAL ERROR RETURN IS TAKEN WITH NO ERROR INDICATED.
;REGISTERS USED:
;
; R0, R1 PASS ARGUMENTS AND ARE ALTERED
CLRTCH: MOV R2,-(SP) ;SAVE REGISTER
MOV R1,R2
BIC #177400,R1 ;GET SENSOR NUMBER
CMP #MAXTCH,R1 ;MUST BE LESS THAN THIS
BGT 1$
MOV #UNKTCH,R0 ;SIGNAL ERROR
BR CTHDNE
1$: ASL R1 ;WANT WORD INDEX
ADD #TCHOFF,R1 ;GET PTR TO TRIGGER ON OFF STATE
BIT #400,R2 ;CHECK IF TRIGGER USING ON OR OFF STATE
BEQ .+6
ADD #TCHON-TCHOFF,R1 ;GET PTR TO TRIGGER ON ON STATE
;TAKE TOUCH EVENT OFF EVENT LIST
INCB TCHUP ;LOCK OUT LEVEL 6 CODE FROM USING EVENT LIST
2$: TST (R1) ;END OF LIST?
BEQ CTEOL ;BRANCH IF NO EVENT MATCH FOUND
MOV (R1),R2 ;ELSE GET POINTER TO EVENT
CMP R0,(R2)+ ;MATCHING EVENT?
BEQ GOTEVT ;BRANCH IF WE FOUND IT
MOV R2,R1 ;ELSE GET POINTER TO NEXT EVENT ON LIST
BR 2$ ;CHECK NEXT EVENT
GOTEVT: MOV (R2),(R1) ;SKIP OVER REQUESTED EVENT
CLR -2(R2) ;FREE EVENT SLOT
DEC ACTNUM ;INDICATE ONE LESS EVENT PENDING
CTEOL: DECB TCHUP ;UNLOCK LEVEL 6 CODE
CLR R0 ;INDICATE NO ERRORS
;EXIT CLEANLY
CTHDNE: MOV (SP)+,R2 ;RESTORE REGISTER
RTS PC
;END OF "CLRTCH"
; SMPTCH, STPTCH - initiates & terminates updating of the touch sensor readings
;"SMPTCH" ENSURES THAT THE TOUCH SENSOR STATUS WORDS WILL BE UPDATED PERIODicLY
;WHETHER OR NOT ANY TOUCH SENSOR EVENTS ARE PENDING. A SAMPLE CALLING SEQUENCE
;TO THIS ROUTINE FOLLOWS:
;
; JSR PC,SMPTCH ;NO ARGUMENTS
;
;SUCCESS IS ALWAYS GUARANTEED, HENSE NO ERROR CODE IS RETURNED
;REGISTERS USED: NONE
SMPTCH: INCB TCHUP ;LOCKOUT LEVEL 6 CODE
BIS #RUNTCH,TCHUP ;TELL LEVEL 6 CODE TO RUN CONTINUOUSLY
BIS #TCHSCH,TCHUP ;INDICATE LEVEL 6 CODE SCHEDULED
DECB TCHUP ;UNLOCK LEVEL 6 CODE
RTS PC
;"STPTCH" RETURNS THE LEVEL 6 CODE TO MODE OF OPERATION OF ONLY RUNNING IF
;THERE IS A PENDING TOUCH SENSOR EVENT IN THE EVENT LIST. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; JSR PC,STPTCH ;NO ARGUMENTS
;
;SUCCESS IS ALWAYS GUARANTEED, HENSE NO ERROR CODE IS RETURNED
;REGISTERS USED: NONE
STPTCH: BIC #RUNTCH,TCHUP ;RETURN TO STANDARD MODE
RTS PC
;END OF "SMPTCH" AND "STPTCH"
; TCHLV6 - level 6 code for touch sensors
;THIS ROUTINE UPDATES THE STATUS WORDS FOR THE TOUCH SENSORS ( TCHSTS ) WHEN
;IT IS RUN SO LONG AS THE LOCKOUT BYTE OF THE STATUS WORD ( TCHUP ) IS ZERO.
;IF UPDATING IS PERMITTED, ANY EVENTS IN THE EVENT LIST WHOSE CONDITION IS
;NOW TRUE ARE SIGNALLED. THIS ROUTINE will continue to be called by "servo"
;IF AT LEAST ONE EVENT IS PENDING OR IF THE "RUNTCH" BIT OF THE "TCHUP" WORD
;IS ON.
;IF NOT LOCKED OUT, UPDATE SENSORS AND SIGNAL TRUE CONDITION EVENTS
TCHLV6: TSTB TCHUP ;CHECK IF LOCKED OUT
BNE endtch ;LOCKED OUT, JUST return to "servo"
MOV #TCHADL,AC ;PTR TO ADC CHANNELS TO READ
MOV #TCHVAL,BC ;PTR TO ARRAY TO STORE ADC READINGS
MOV #TCHOFF,CC ;PTR TO "OFF" CONDITION EVENTS
MOV #TCHON,DC ;PTR TO "ON" CONDITION EVENTS
MOV #TCHSTS,EC ;PTR TO ON/OFF STATUS WORDS FOR CHANNELS
MOVB (AC)+,ADCCHN ;START ADC WORKING ON FIRST CHANNEL
MOV #TLVEXT,-(SP) ;SAVE ADDRESS OF EXIT SECTION
TLV6LP: TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,(BC) ;SAVE ADC READING
MOVB (AC),ADCCHN ;START CONVERTING NEXT CHANNEL
CMP SENZ0-TCHVAL(BC),(BC)+ ;CHECK SENSOR VALUE
BLT 1$ ;BRANCH IF SENSOR ABOVE ZERO LEVEL
CLR (EC)+ ;INDICATE SENSOR OFF
MOV (CC),DAT ;WANT PTR TO SIGNAL "OFF" CONDITION EVENTS
CLR (CC)+ ;INDICATE NO MORE EVENTS PENDING ON THIS CONDITION
TST (DC)+ ;UPDATE "ON" EVENT POINTERS
BR 2$
1$: MOV #1,(EC)+ ;INDICATE SENSOR ON
MOV (DC),DAT ;WANT PTR TO "ON" CONDITION EVENTS
CLR (DC)+
TST (CC)+ ;UPDATE "OFF" EVENT POINTER
2$: TST DAT ;CHECK IF ANY EVENTS PENDING
BEQ NXTSEN ;IF NONE, SKIP TO NEXT SENSOR
3$: MOV (DAT),-(SP) ;WE WILL SIGNAL THIS LATER SINCE THE KERNEL WOULD
MOV #SGNEVT,-(SP) ; CLOBBER OUR REG. IF WE DID IT NOW
CLR (DAT)+ ;FREE EVENT LIST SLOT
DEC ACTNUM ;INDICATE ONE LESS EVENT PENDING
MOV (DAT),DAT ;GET PTR TO NEXT EVENT ON LIST
BNE 3$ ;REPEAT FOR ALL EVENTS ATTACHED TO THIS CONDITION
NXTSEN: TSTB (AC)+ ;CHECK IF END OF ADC LIST
BPL TLV6LP ;REPEAT TILL DONE
BR TSTSIG
SGNEVT:
EVSIG ;SIGNAL AN EVENT THAT OCCURRED
TSTSIG: RTS PC ;THIS BRANCHS TO SGNEVT IF WE SAVED ANY EVENTS TO
TLVEXT: ; SIGNAL, OTHERWISE IT COMES HERE
TST ACTNUM ;CHECK IF ANY EVENTS STILL PENDING
BNE endtch ;BRANCH IF STILL MORE
BIT #RUNTCH,TCHUP ;ELSE CHECK IF WE ARE TO RUN CONTINUOUSLY
bne endtch
tlvkil: bic #tchsch,tchup ;IF NOT, NO MORE TO DO, KILL LEVEL 6 CODE
;INDICATE LEVEL 6 NOT SCHEDULED AGAIN
endtch: rts pc
;TOUCH SENSOR SERVO DATA BLOCK
DATA
TCHUP: 0 ;LEVEL 6 STATUS BITS
ACTNUM: 0 ;NUMBER OF PENDING EVENTS
;DATA FOR USING BINARY FINGERS
.IFZ FFING
TCHADL: .BYTE 0,1,377,0 ;TOUCH SENSOR A/D CHANNELS
SENZ0: 450. ;TRANSITION READING FOR BINARY FINGERS
450.
.ENDC
;DATA FOR USING FORCE SENSING FINGERS
.IFNZ FFING
TCHADL: .BYTE 34.,35.,377,0 ;TOUCH SENSOR A/D CHANNELS
SENZ0: 200 ;TRANSITION READING FOR FORCE FINGERS
200
.ENDC
;TCHVAL TO LSTUSE IS ZEROED BY "INTARM"
TCHVAL: .BLKW MAXTCH ;A/D TOUCH SENSOR READINGS
TCHSTS: .BLKW MAXTCH ;TOUCH SENSOR LOGICAL STATUS ( TRUE OR FALSE )
TCHON: .BLKW MAXTCH ;PTR TO EVENTS TO TRIGGER IF TOUCH SENSOR ON
TCHOFF: .BLKW MAXTCH ; " " " " " " " " OFF
EVTLST: .BLKW TCHBLK*2;STORAGE FOR EVENTS TO BE TRIGGERED
LSTUSE: TCHBLK*4 ;REL. POINTER TO LAST SLOT USED IN EVENT LIST
CODE
;END OF "TCHLV6"
;FORCE - force sensing and compliance subroutines
FRATE ==6 ;NUMBER OF TIMES FRCLV6 IS EXECUTED BEFORE A CALL TO FBACK
WRSPLS ==40 ;FORCE WRIST PULSE BIT, TO RESET MULTIPLEXER
WRSCHN ==66 ;FORCE WRIST ADC CHANNEL
FRCBLK ==15. ;NUMBER OF FORCE JOB THAT CAN BE PENDING
;THE FOLLOWING MECHANISM BITS INDICATE WHICH PHYSICAL DEVICE IS BEING
;USED:
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
;THE FOLLOWING BITS ARE USED DURING CALLS TO THE FORCE SENSING SYSTEM AND
;BY THE FORCE SENSING SYSTEM ITSELF TO SPECIFY RUN-TIME OPTIONS.
FTABLE ==400 ;FORCE TRANS (C) IN TABLE COORDINATES
FHAND ==0 ; " " " " HAND COORDINATE SYSTEM
XFORCE ==0 ;FORCE IN X DIRECTION OF C
YFORCE ==1000 ; " " Y " " "
ZFORCE ==2000 ; " " Z " " "
XMOMENT ==3000 ;MOMENT ABOUT X DIRECTION OF C
YMOMENT ==4000 ; " " Y " " "
ZMOMENT ==5000 ; " " Z " " "
FSTOP ==10000 ;IN ADDITION TO SPROUTING JOB, STOP ARM
SIGGE ==100000;START JOB IF FORCE ≥ SPECIFIED VALUE
SIGLT ==0 ; " " " " < " "
SIGMAG == 20000;TEST ONLY MAGNITUDE OF FORCES
;SUBR TO VERIFY SAME ARM SPECIFIED BY FORCE ROUTINES
SAMARM: MOV R0,R3 ;SAME ARM SPECIFIED?
BIC #-1#BLUARM#YELARM,R3
BIT R3,@FCARAY
BNE 1$
MOV #WRGARM,R0 ;NO, SIGNAL ERROR
SEV
1$: RTS PC
; SETC - initializes force sensing/compliance system
.IFZ DIAGY
;THIS ROUTINE IS USED FOR SPECIFYING THE FORCE COORDINATE SYSTEM TOGETHER
;WITH THE MANIPULATOR THAT IS TO BE FORCE SERVOED AND FOR INITIALIZING
;THE FORCE SENSING/COMPLIANCE ROUTINES. IT SHOULD BE CALLED AT LEAST
;ONCE BEFORE THE START OF EACH MOTION THAT REQUIRES FORCE SENSING/COMPLIANCE.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;BLUARM/YELARM AND TABLE/HAND BITS
; MOV #C,R1 ;PTR TO FORCE TRANS
; JSR PC,SETC
;
;IF THE FORCE SENSING SYSTEM IS CURRENTLY ACTIVE AND THE ARM SPECIFIED
;IN R0 IS DIFFERENT FROM THE ARM CURRENTLY IN USE, THIS INSTRUCTION IS
;ABORTED AND AN ERROR CODE IS RETURNED IN R0, ELSE R0 IS SET TO ZERO.
;
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE GARBAGED
SETC: MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
BIT #RUN,FSTAT ;FORCE SYSTEM ACTIVE?
BEQ 1$ ;NO
JSR PC,SAMARM ;YES, SAME ARM?
BVS SETCDN ;NO
1$: MOV FCARAY,R2 ;PTR TO CURRENT C TRANS AND BITS
MOV NEXTC(R2),R2 ;PTR TO NEXT TRANS/BITS DATA AREA
MOV R2,R3
MOV R0,(R2)+ ;SAVE ARM/MODE BITS
MOV #12.*2,R0 ;TRANSFER C TRANSFORM
MOV (R1)+,(R2)+
SOB R0,.-2
MOV R3,FCARAY ;START USING NEW C ARRAY AND BITS
mov #6,r2
mov #biasf,r3
6$: clrf <6*4>(r3) ;ZERO ALL BIAS FORCES
clrf (r3)+
sob r2,6$
SETCDN: MOV (SP)+,R5
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;END OF "SETC"
; FRCSIG - initializes job starting based upon a force reading
;THIS RTN. IS USED FOR SPROUTING JOBS THAT ARE TO BE TRIGGERED ACCORDING
;TO THE VALUE OF A FORCE COMPONENT. THE SPROUTING CAN TAKE PLACE WHEN
;A FORCE LEVEL EITHER EXCEEDS OR DROPS BELOW A SPECIFIED MAGNITUDE. THE ONLY
;CONSTRAINTS ARE THAT THE FORCE COMPONENT TO BE CHECKED MUST COINCIDE WITH
;ONE OF THE CARDINAL AXES OF THE CURRENT FORCE TRANSFORM( SEE "SETC") AND THE
;ARM SPECIFIED MUST BE THE SAME AS THAT SPECIFIED IN THE LAST "SETC" CALL. AS
;MANY AS "FRCBLK" DIFFERENT JOBS CAN BE WAITING FOR VARIOUS FORCE LEVELS. THE
;EVALUATION OF THE CURRENT FORCE LEVELS ARE DONE AT LEVEL 6 AND A SUBSIDIARY
;JOB IS RUN AT LEVEL 5. BOTH OF THESE JOBS ARE RUN ONLY WHEN SPROUTING IS
;PENDING OR SOME FORCE COMPLIANCE TASK IS REQUESTED.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SEE BELOW
; MOV #PDB,R1 ;PDB OF JOB TO BE TRIGGERED ON FORCE
; MOV #ADDR,R2 ;STARTING ADDR OF SPROUTING JOB
; LDF VAL,AC0 ;FORCE VALUE IN OZ OR OZ-IN
; JSR PC,FRCSIG ;***WARNING: NEVER CALL THIS ROUTINE
; ; ABOVE CPU LEVEL 4***
;
;THE CONTENTS OF THE "BITS" IN R0 ARE USED FOR SPECIFYING ONE OPTION FROM
;EACH OF THE FOLLOWING GROUPS:
;
; ARM: YELLOW/BLUE
; FORCE COMPONENT TO CHECK: FORCE X/Y/Z MOMENT X/Y/Z
; START JOB IF: FORCE ≥/< SPECIFIED VALUE
; STOP ARM WHEN JOB STARTED: YES/NO
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;QUEUING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0,R1,R2,AC0 PASS ARGUMENTS AND R0 IS ALTERED
FRCSIG: MOV R3,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS FRCDNE ;NO
MOV R0,R3 ;GET PTR INTO FORCE COMPONENT LIST
BIC #170777,R3
SWAB R3
;PUT FORCE JOB IN QUEUE
INCB FSTAT+1 ;LOCK OUT LEVEL 6 CODE FROM USING JOB LIST
MOV FRCPTR,R4 ;ANY FREE JOB BLOCKS?
BNE 1$ ;YES
MOV #TOMFRC,R0 ;NO, SIGNAL ERROR AND ABORT
BR 2$
1$: MOV (R4),FRCPTR ;TAKE BLOCK OUT OF FREE CHAIN
MOV XFPTR(R3),(R4) ;PUT BLOCK IN COMPONENT CHAIN
MOV R4,XFPTR(R3)
TST (R4)+ ;SAVE JOB DATA IN BLOCK
BIT #SIGMAG,R0 ;MAGNITUDE ONLY?
BEQ 3$ ;NO
ABSF AC0 ;YES, ALLOW ONLY POSITIVE THRESHOLDS
3$: STF AC0,(R4)+ ;FORCE VALUE
BIC #FTABLE,R0 ;ONLY HAND COORDS AVAILABLE
MOV R0,(R4)+ ;BITS
MOV #3,(R4)+ ;TEST COUNTER
MOV R2,(R4)+ ;SPROUTING JOB
MOV R1,(R4)
INC FRCNUM ;INDICATE ONE MORE JOB PENDING
CLR R0 ;INDICATE NO ERRORS
2$: DECB FSTAT+1 ;ALLOW LEVEL 6 CODE TO USE JOB LIST
;EXIT CLEANLY
FRCDNE: MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
RTS PC
;END OF "FRCSIG"
; FRCOFF - takes a queued job off of the force signal list
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "FRCSIG". A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SAME TWO ARGS AS IN "FRCSIG"
; MOV #PDB,R1
; JSR PC,FRCOFF
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;DELETING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0, R1 PASS ARGUMENTS AND ARE ALTERED
FRCOFF: MOV R3,-(SP) ;SAVE REGISTER
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 5$ ;NO
BIC #170777,R0 ;GET PTR INTO FORCE COMPONENT LIST
SWAB R0
ADD #XFPTR,R0 ;PTR TO SPECIFIED COMPONENT LIST
INCB FSTAT+1 ;LOCK OUT LEVEL 6 CODE FROM USING JOB LIST
MOV (R0),R3 ;FIRST JOB IN QUEUE
;TAKE FORCE JOB OUT OF QUEUE
1$: CMP 14(R3),R1 ;FOUND RIGHT PDB?
BEQ 3$ ;YES
MOV (R3),R3 ;NEXT JOB IN QUEUE
BNE 1$
MOV #1,R0 ;COULDN'T FIND REQUESTED JOB
BR 4$ ;NO MORE JOBS TO CHECK
3$: MOV (R3),(R0) ;REMOVE JOB FROM QUEUE
MOV FRCPTR,(R3)
MOV R3,FRCPTR
DEC FRCNUM ;INDICATE ONE LESS JOB PENDING
CLR R0 ;SIGNAL NO ERROR
4$: DECB FSTAT+1 ;ALLOW LEVEL 6 CODE TO USE JOB LIST
5$: MOV (SP)+,R3
RTS PC
;END OF "FRCOFF"
;CLRFRC - REMOVES ALL FORCE SENSING JOBS ASSOCIATED WITH BLUARM OR YELARM
;R1 SHOULD CONTAIN #BLUARM OR #YELARM TO INDICATE WHICH JOBS TO PURGE
;NO ERROR RETURN IS MADE. JOB LIST SHOULD BE LOCKED OUT DURING CALL
;
; SAMPLE CALL: INCB FSTAT+1 ;LOCK JOBLIST
; MOV #BLUARM,R1
; JSR PC,CLRFRC
; DECB FSTAT+1 ;UNLOCK
;
CLRFRC: MOV #6,R2 ;6 COMPONTENT LISTS TO CHECK
MOV #XFPTR,R4
1$: MOV R4,R0 ;POINTER TO LIST POINTER
BR 3$
2$: MOV (R0),R0 ;POINTER TO NEXT BLOCK
3$: MOV (R0),R3 ;POINTER TO NEXT BLOCK
BEQ 6$ ;R3 POINTS TO VALID BLOCK?
BIT R1,6(R3) ;YES - IS IT FOR CORRECT ARM?
BEQ 2$
MOV (R3),(R0) ;YES - REMOVE BLOCK
MOV FRCPTR,(R3)
MOV R3,FRCPTR
DEC FRCNUM
BR 3$
6$: TST (R4)+ ;POINT TO NEXT LIST POINTER
SOB R2,1$
MOV #6,R2 ;ZERO ALL BIAS FORCES
MOV #BIASF,R3
7$: CLRF 30(R3) ;clear bias torque
CLRF (R3)+ ;clear bias force
SOB R2,7$
RTS PC
;END OF "CLRFRC"
; BISON - sets up bias force of a given magnitude and direction
;
;THIS IS JUST LIKE "FRCSIG" EXCEPT THAT INSTEAD OF QUEUING JOBS WAITING
;ON FORCE LEVELS, THIS ROUTINE INITIALIZES A BIAS FORCE IN A GIVEN
;CARDINAL DIRECTION OF THE CURRENT FORCE TRANSFORMATION ( SEE "SETC" )
;AND AT A GIVEN FORCE MAGNITUDE. (routine currently defaults to hand frame)
;AS WITH "FRCSIG" THE SPECIFIED ARM
;MOST COINCIDE WITH THE ARM SPECIFIED IN THE LAST CALL TO "SETC" OR
;AN ERROR MESSAGE IS RETURNED. NATURALLY, ONLY ONE BIAS LEVEL
;CAN BE SET IN ANY SINGLE DIRECTION AND SO ONLY THE MOST RECENT FORCE
;LEVELS ARE RETAINED. THIS ROUTINE SHARES A LEVEL 6 AND A SUBSIDIARY
;LEVEL 5 JOB WITH "FRCSIG".
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SEE BELOW
; LDF VAL,AC0 ;FORCE VALUE IN OZ OR OZ-IN
; JSR PC,BISON ;***WARNING: NEVER CALL THIS ROUTINE
; ; ABOVE CPU LEVEL 4***
;
;THE CONTENTS OF THE "BITS" IN R0 ARE USED FOR SPECIFYING ONE OPTION FROM
;EACH OF THE FOLLOWING GROUPS:
;
; ARM: YELLOW/BLUE
; FORCE COMPONENT TO APPLY: FORCE X/Y/Z MOMENT X/Y/Z
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR
;OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0,AC0 PASS ARGUMENTS AND R0,R1 ARE ALTERED
BISON: MOV R3,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 1$
BIC #170777,R0 ;PTR INTO COMPLIANCE LIST
SWAB R0
ASL R0
STF AC0,biasf(R0) ;SAVE COMPLIANCE LEVEL
CLR R0 ;INDICATE NO ERRORS
1$: MOV (SP)+,R3
RTS PC
;END OF "BISON"
; BISOFF - turns off force compliance in a specified direction
;
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "BISON". A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SAME ARG AS IN "COMPLY"
; JSR PC,BISOFF
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;DELETING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;(OLD CMPOFF ROUTINE FLUSHED TO OLDFRC.PAL[1,JKS] ON 14-AUG-79)
BISOFF: MOV R3,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 1$
BIC #170777,R0 ;PTR INTO COMPLIANCE LIST
SWAB R0
ASL R0
CLRF biasf(R0) ;CLEAR BIAS FORCE
CLR R0 ;INDICATE NO ERRORS
1$: MOV (SP)+,R3
RTS PC
;END OF BISOFF
; FRCLV6 - level 6 code for force sensing and compliance
;AT THE START OF EXECUTION OF THIS ROUTINE, DAT ← #XFPTR
FRCLV6: TST @FINUSE ;CATASTROPHIC ERROR TERMINATION?
BPL .+6
JMP KILFRC ;YES
TST STFDAT ;HAVE WE ALREADY READ THE GAGES?
BNE 1$
JSR PC,REPS ;READ GAGES
1$: TST GDATA ;ARE WE GATHERING?
BEQ 2$ ;NO
JSR PC,RNS ;YES RESOLVE AND STORE INFO.
2$: TST FRCNUM ; even if no sensing to do we still must start
BEQ resfrc ; FBACK periodically
BIT #177400+FIRST,FSTAT;1ST PASS OR LOCKED OUT FROM USING JOB LIST?
BNE RESFRC ;YES
MOV #STDCAL,BC ;POINT TO WRIST CALIBRATION MATRIX
MOV #6,EC ;# OF FORCE COMPONENTS TO CHECK
MOV #FRCEXT,-(SP) ;"RTS" THIS TO EXIT
FRCLOP: MOV (DAT),DC ;ANY JOBS PENDING ON THIS COMPONENT?
BEQ NOFJOB
MOV #EPSF,AC ;point to table of strain gage readings
CLRF AC0
MOV #8.,CC ;COMPUTE force value = STDCAL*EPSF
1$: LDF (AC)+,AC1 ;get gage reading
MULF (BC),AC1 ;JACOBIAN COMPONENT
ADD #24.,BC
ADDF AC1,AC0 ;DOT PRODUCT
SOB CC,1$
SUB #<8.*6*4>,BC ;POINT TO NEXT COL
MOV DAT,CC ;CHECK IF ANY PENDING JOBS TO START UP
CHKJOB: TST (DC)+
STF AC0,AC1
BIT #SIGMAG,4(DC) ;CHECKING ONLY MAGNITUDE?
BEQ 3$ ;NO
ABSF AC1 ;GET MAGNITUDE
3$: CMPF (DC)+,AC1 ;CHECK VALUE
TST (DC)+ ;SPROUT ON FORCE ≥ VAL OR < VAL?
BPL 1$
CFCC ;VAL ≤ FORCE?
BLE 2$ ;YES
BR 5$ ;NO
1$: CFCC ;VAL > FORCE?
BLE 5$ ;NO
2$: DEC (DC) ;FILTER, DONT STOP UNLESS TEST OK 3 TIMES
BGT 6$
BIT #FSTOP,-(DC) ;STOP ARM MOTION?
BEQ .+10
BIS #100000,@FINUSE ;YES
CMP (DC)+,(DC)+
MOV #USRDM,-(SP) ;WE WILL SPROUT THIS JOB LATER SINCE THE KERNEL
MOV (DC)+,-(SP) ; WOULD CLOBBER OUR REG. IF WE DID IT NOW
MOV (DC),-(SP) ; WOULD CLOBBER OUR REG. IF WE DID IT NOW
MOV #SPRTIT,-(SP)
MOV (CC),DC ;RETURN THE DATA BLOCK TO FREE STORAGE LIST
MOV (DC),(CC)
MOV FRCPTR,(DC)
MOV DC,FRCPTR
DEC FRCNUM ;INDICATE ONE LESS JOB PENDING
BR 7$
5$: MOV #3,(DC) ;RESET TEST COUNT
6$: MOV (CC),CC
7$: MOV (CC),DC ;CHECK NEXT JOB IN FORCE LIST
BNE CHKJOB
NOFJOB: TST (DAT)+ ;ON TO NEXT FORCE COMPONENT
ADD #4,BC ;POINT TO NEXT COL
DEC EC
BGT FRCLOP
BR SIGFRC
SPRTIT: FORK ;SPROUT FORCE JOB
SIGFRC: RTS PC ;THIS BRANCHES TO SPRTIT IF WE SAVED ANY JOBS TO
FRCEXT: ; SPROUT, OTHERWISE IT COMES HERE
;EXIT FROM LEVEL6 JOB ONE OF THREE WAYS:
RESFRC: BIT #FIRST,FSTAT ;1ST TIME THROUGH?
BNE 1$ ;YES
DEC BCKTME ;TIME TO SCHEDULE BACKGROUND JOB?
BGT FRCFIN ;NO
1$: BIT #BACKG,FSTAT ;BACKG JOB NOT YET THROUGH RUNNING?
BNE 2$ ;YES, BIG TROUBLE
MOV #FRATE,BCKTME ;RUN BACKGROUND JOB EVERY 5TH TIME
BIC #FIRST,FSTAT
FORK #FRCPD5,#FBACK,#USRDM ;START UP BCK JOB
br frcfin
2$: BIS #100000,@FINUSE ;SIGNAL CAT. ERROR
INC BOVLCK ;indicate overrun condition
FORK #BOVPDB,#BOVER,#USRDM ;SIGNAL BACKG. JOB NOT DONE
KILFRC: CLRB FSTAT ;CLEAR ALL RUN FLAGS
FRCFIN: rts pc ;return (to servo)
;BACKGROUND JOB OVER-RUN ERROR MESSAGE JOB
BOVER:
.IFZ STDALN
ALERR BOVERM ;TELL OPERATOR, BACKGROUND JOB TOOK TOO LONG
.IFF
MOV #BOVERM,SG
JSR PC,TYPSTR
.ENDC
DEC BOVLCK ;OPEN LOCK
DISMISS
DATA
BOVPDB: PDBLK 6,30
BOVERM: .ASCIZ /BACKGROUND FORCE SYSTEM JOB TOOK TOO LONG TO EXECUTE.
YOU'RE IN BIG TROUBLE NOW, CALL FOR HELP.
/
.EVEN
CODE
;END OF FRCLV6
; FBACK - level 5 force sensing background job
;THE FOLLOWING JOB IS STARTED AFTER EVERY 3RD EXECUTION OF THE LEVEL 6
;FORCE SENSING JOB, "FRCLV6". IT RECOMPUTES JACOBIAN AND STIFFNESS MATRICES
:AS NEEDED FOR FORCE SENSING, STIFFNESS CONTROL AND FORCE DATA GATHERING.
;THIS ROUTINE ;SHOULD ONLY BE EXECUTED BY DOING EITHER A "FORK"
;OR A "SCHEDU" THROUGH THE MONITOR.
FBACK: BIS #BACKG,FSTAT ;INDICATE BACKGROUND JOB RUNNING
TST STFDAT ;CHECK IF FORCE SERVOING
BEQ FBCKDN ;BR IF NOT
MOV #IMAT,R0 ;POINTER TO FORCE TRANS
MOV #THPTR,R1 ;POINT TO LIST OF ALL JOINT ANGLES
MOV #BLUARM+FHAND,R2;INDICATE BLUE ARM IN HAND COORDS
MOV #JC,R3 ;POINT TO RESULT MATRIX
; JSR PC,JACOB ;SHOULD BE UNCOMMENTED IF CHANGING ORIENTATIONS DURING MOVES
JSR PC,GJTCAL ;COMPUTE JOINT TORQUE CALIBRATION MAT.
MOV RELX,R1 ;POINT TO RELATIVE XFORM TO STIFFNESS CENTER
JSR PC,GETKTH ;COMPUTE STIFFNESS MATRIX
;need to compute jacob in sta coords if so indicated
MOV #JC,BIASJA ;ASSUME IN BIAS FORCES IN HAND COORDS
; BIT #STACOORDS,SOMEWHERE
; BEQ 7$
; MOV #IMAT,R0 ;POINTER TO FORCE TRANS
; MOV #THPTR,R1 ;POINT TO LIST OF ALL JOINT ANGLES
; MOV #BLUARM+FSTA,R2 ;INDICATE BLUE ARM IN STATION COORDS
; MOV #JCSTA,R3 ;POINT TO RESULT MATRIX
; JSR PC,JACOB ;COMPUTE JACOBIAN
; MOV #JCSTA,BIASJA ;USE JACOBIN IN STA CCORDS
;COMPUTE BIAS TORQUES FROM BIAS FORCE
7$: MOV #BIAST,R0
MOV BIASJA,R1
MOV #6,R4
10$: MOV #BIASF,R2
MOV #6,R3
CLRF AC0
11$: LDF (R2)+,AC1
MULF (R1)+,AC1
ADDF AC1,AC0
SOB R3,11$
STF AC0,(R0)+
SOB R4,10$
; MOV #BTH,R0 ;COMPUTE CURRENT DYNAMIC COEF
; MOV #BCI,R1 ;UPDATE SERVO DATA BLOCKS
; MOV #BLUARM,R2 ;INDICATE BLUE ARM
; JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
;EXIT CLEANLY
FBCKDN: BIC #BACKG,FSTAT ;BACKGROUND JOB DONE
DISMIS
.IFF ;THIS MATCHES THE ".IFZ DIAGY" AT THE START OF THE FORCE RTNS.
FRCLV6:
.ENDC
;END OF "FBACK"
;WRIST SENSOR definitions
;BITS INDICATING WHICH FORCES ARE TO BE GATHERED ARE DEFINED AS FOLLOWS:
XFBIT == 1
YFBIT == 2
ZFBIT == 4
XMBIT == 10
YMBIT == 20
ZMBIT == 40
TBLBIT == 10000 ;CONVERT READINGS TO TABLE COORDS (DEFAULT IS HAND COORDS)
;BITS INDICATING WHICH JOINTS TO GATHER RESOLVED TORQUES FOR ARE:
T1BIT == 100 ;JOINT 1
T2BIT == 200 ;2
T3BIT == 400 ;3
T4BIT == 1000 ;4
T5BIT == 2000 ;5
T6BIT == 4000 ;6
; SETBAS - sets the base readings for the force wrist
;THIS PROCEDURE IS USED TO TAKE AN INITIAL SET OF READINGS TO BE
;USED AS AN OFFSET IN RESOLVING THE SCHEINMAN FORCE SENSING WRIST.
;THE ONLY ARGUMENT REQUIRED BY THIS ROUTINE IS A POINTER
;TO A INTEGER ARRAY 8 WORDS LONG. AFTER EXECUTION, THE STRAIN
;GAGE READINGS ARE RETURNED IN THIS ARRAY AS WELL AS BEING LEFT
;IN AN INTERNAL ARRAY WHICH IS USED BY THE FORCE RESOLVING
;ROUTINE "WRIST". IF YOU DON'T NEED THE STRAIN GAGE READINGS,
;THE REGISTER WHICH NORMALLY CONTAINS THE ARRAY POINTER SHOULD BE
;SET TO ZERO. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #SAVEBASE,R0 ;SET TO 0 IF NOT NEEDED
; JSR PC,SETBAS
;
;NO ERROR MESSAGES ARE RETURNED BY THIS ROUTINE.
;REGISTERS USED:
; R0 PASSES AN ARGUMENT AND IS NOT ALTERED
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
SETBAS: INC BASLCK ;CHECK IF LOCKED OUT
BEQ 1$ ;BRANCH IF FREE TO GO ON
DEC BASLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR SETBAS
1$: MOV R0,BASPTR ;SET POINTER TO RETURN BASE VALUES
BNE .+10 ;DONT RETURN VALUES?
MOV #BASRDG,BASPTR
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),BASEVT ;SAVE EVENT NUMBER FOR LEVEL 6 CODE
MOV #1,GOBAS ;REQUEST LVL 6 SERVICE
EVWAIT (SP) ;WAIT FOR THE LEVEL 6 JOB TO FINISH
EVKIL
CLR GOBAS ;REMOVE REQ. FOR LVL 6 SERVICE
DEC BASLCK ;RELEASE CODE FOR FUTURE USE
RTS PC ;RETURN
;LEVEL 6 CODE FOR "SETBAS"
BASLV6: MOV AC,-(SP) ;SAVE REGS.
MOV BC,-(SP)
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOV BASPTR,DAT
MOV DAT,-(SP) ;SAVE POINTER TO BASRDG
MOV #8.,DC
2$: CLR (DAT)+ ;CLEAR BASRDG FOR ACUMULATING READINGS
SOB DC,2$
MOV #4,DC
3$: MOV #30,BC ;GET FIRST WRIST CHANNEL ADDRESS
MOVB BC,ADCCHN ;START FIRST CONVERSION
MOV #10,CC ;SET COUNTER TO READ 8. CHANNELS
MOV (SP),DAT ;GET FIRST BASRDG ADDRESS
1$: TSTB ADCSR ;WAIT FOR CONVERSION
BPL .-4
ADD ADCVAL,(DAT)+ ;SAVE NEW READING
TSTB (BC)+ ;POINT TO NEXT CHANNEL
MOVB BC,ADCCHN ;START CONVERTING NEXT CHANNEL
SOB CC,1$ ;REPEAT UNTIL DONE 8 SENSORS
SOB DC,3$ ;REPEAT READINGS 4 TIMES
MOV BASPTR,DC ;GET PTR TO USER STORAGE ARRAY
MOV (SP)+,DAT
MOV #8.,AC
4$: ASR (DAT) ;DIVIDE BY 4
ASR (DAT)
MOV (DAT)+,(DC)+ ;SAVE DATA FOR USER
SOB AC,4$
MOV (SP)+,EC ;RESTORE REGS.
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,BC
MOV (SP)+,AC
5$: EVSIG BASEVT ;SIGNAL END OF LEVEL 6 CODE
RTS PC
;LOCAL STORAGE AREA
DATA
BASEVT: .BLKW 1 ;LEVEL 6 CODE COMPLETION EVENT
BASPTR: .BLKW 1 ;PTR TO ARRAY TO STORE READINGS IN FOR USER
BASRDG: .BLKW 8. ;ARRAY TO STORE READINGS FOR "WRIST"
BASLCK: -1 ;-1 → SETBAS READY FOR USE, 0 → LOCKED UP
CODE
;END OF "SETBAS"
; WRIST - reads and resolves the force wrist readings
;THIS PROCEDURE IS USED TO READ AND RESOLVE THE FORCE WRIST STRAIN
;GAGE READINGS INTO EQUIVALENT FORCE AND MOMENT COMPONENTS. THE
;ARGUMENTS REQUIRED BY THIS ROUTINE ARE A POINTER TO A 6x8 CALIBRATION
;TABLE USED TO CONVERT THE READINGS TO FORCES/MOMENTS AND A POINTER
;TO A 1x6 ARRAY INTO WHICH THE COMPUTE FORCE/MOMENT COMPONENTS ARE STORED.
;IF THE STANDARD CALIBRATION MATRIX TO RESOLVE FORCES AND MOMENTS
;IN THE REFERENCE FRAME OF THE FORCE WRIST IS DESIRED, THE POINTER
;TO THE 6x8 CAN BE SET TO ZERO. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #CALTAB,R0 ;SET TO 0 IF NOT NEEDED
; MOV #FORCES,R1 ;PTR TO TABLE OF RESULTS
; JSR PC,WRIST
;
;NO ERROR MESSAGES ARE RETURNED BY THIS ROUTINE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE NOT ALTERED
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
WRIST: INC WSTLCK ;CHECK IF LOCKED OUT
BEQ 1$ ;BRANCH IF FREE TO GO ON
DEC WSTLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR WRIST
1$: MOV R0,CALPTR ;SET POINTER TO CALIBRATION DATA
BNE .+10
MOV #STDCAL,CALPTR ;USE STANDARD CALIBRATION AS DEFAULT
MOV R1,FORPTR ;STORE COMPUTED F/M'S IN HERE
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),WSTEVT ;SAVE EVENT NUMBER FOR LEVEL 6 CODE
MOV #1,GOWST ;REQUEST LVL 6 SERVICE
EVWAIT (SP) ;WAIT FOR THE LEVEL 6 JOB TO FINISH
EVKIL
CLR GOWST ;REMOVE REQUEST FOR LVL 6 SERVICE
DEC WSTLCK ;RELEASE CODE FOR FUTURE USE
RTS PC ;RETURN
;LEVEL 6 CODE FOR "WRIST"
WSTLV6: MOV AC,-(SP) ;SAVE REGS.
MOV BC,-(SP)
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOVB #30,ADCCHN ;START ADC CONVERTING FIRST CHANNEL
MOV FORPTR,BC ;INITIALIZE ARRAY TO SAVE DATA IN
MOV #6,AC ;ZERO ALL 6 ELEMENTS
CLRF (BC)+ ;*K COULD PUT XFORMED BASE READINGS HERE
SOB AC,.-2
MOV #31,BC ;POINT TO NEXT WRIST CHANNEL
MOV #10,WSTCNT ;SET COUNTER TO READ 8. CHANNELS
MOV #BASRDG,CC ;POINTER TO BASE SENSOR READINGS
MOV CALPTR,DAT ;POINT TO CALIBRATION TABLE
1$: TSTB ADCSR ;WAIT FOR CONVERSION
BPL 1$
MOV ADCVAL,AC
MOVB BC,ADCCHN ;START CONVERTING NEXT CHANNEL
TSTB (BC)+ ;POINT TO NEXT WRIST CHANNEL
SUB (CC)+,AC ;SUBTRACT BASE READING FROM SENSOR READING
LDCIF AC,AC1 ;SAVE CORRECTED READING
MOV FORPTR,EC ;SAVE SUMS OF PRODUCTS IN HERE
MOV #6,DC ;SIX SUMS IN ALL, ONE FOR EACH FORCE/MOMENT
2$: LDF (DAT)+,AC0 ;MULT ADC READING TIMES CALIBRATION MATRIX
MULF AC1,AC0
ADDF (EC),AC0 ;ACCUMULATE FORCE COMPONENTS
STF AC0,(EC)+
SOB DC,2$ ;REPEAT FOR ALL SIX FORCES/MOMENTS
DEC WSTCNT ;REPEAT FOR ALL EIGHT STRAIN GAGES
BGT 1$
MOV (SP)+,EC ;RESTORE REGS.
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,BC
MOV (SP)+,AC
EVSIG WSTEVT ;SIGNAL END OF LEVEL 6 CODE
RTS PC
;LOCAL STORAGE AREA
DATA
CALPTR: .BLKW 1 ;PTR TO CALIBRATION TABLE
FORPTR: .BLKW 1 ;PTR TO ARRAY TO SAVE RESULTS IN
WSTCNT: .BLKW 1 ;LOOP COUNTER
WSTLCK: -1 ;-1 → WRIST READY FOR USE, 0 → LOCKED UP
WSTEVT: .BLKW 1 ;LEVEL 6 CODE COMPLETION EVENT
;PUT CALIBRATION DATA AT FRONT OF ARM DATA IN AL VERSION OF CODE
.IFZ STDALN
YXX == .
. = ARMDAT
.ENDC
STDCAL: ;CALIBRATION MATRIX: CURRENT TIME AND DATE: 16:25 16DEC78 (ENGLISH)
.FLT2 -.2325081E-2 , -.3083945E-2 , .4604810E-1 , -.5110909E-1 , -.3235031E-2 , .2641738E-2
.FLT2 .9794261E-1 , -.4324512E-2 , -.2915139E-2 , .3185859E-1 , .5522090E-1 , -.3471358E-1
.FLT2 .7339207E-3 , .1606639E-2 , .6804592E-1 , .7163971E-2 , -.4028634E-1 , .5120467E-2
.FLT2 .9573883E-2 , .7935277E-1 , -.5759057E-2 , -.2655063E-1 , .6458532E-2 , -.4813714E-1
.FLT2 -.5962288E-2 , -.1015981E-2 , -.5389340E-1 , -.5217535E-1 , .9762089E-4 , -.3138004E-2
.FLT2 .1011939 , -.1167195E-1 , .2421104E-2 , .3971796E-1 , .5830861E-1 , .4213376E-1
.FLT2 .7793080E-2 , -.7565245E-2 , -.5839136E-1 , .5192415E-2 , -.3359392E-1 , -.2440074E-2
.FLT2 -.9898057E-2 , .7865053E-1 , .3517145E-2 , -.2578878E-1 , -.5119964E-2 , .4037705E-1
.IFZ STDALN
. = YXX
.ENDC
CODE
;END OF "WRIST"
;GATHER - collects force data
CODE
;THIS PROCEDURE INITALIZES THE DATA GATHERING SYSTEM. THE FORCE WRIST IS
;READ AT 60 Hz AND SELECTED FORCES ARE STORED IN AN ARRAY. BITS IN ANY
;MEANINGFUL COMBINATION IN R0 DETERMINE WHICH FORCES ARE TO BE RECORDED
;ACCORDING TO THE DEFINITIONS ON PAGE 47 ( ONLY HAND COORDS ARE VALID NOW ).
;R1 SHOULD CONTAIN THE ADDRESS OF A POINTER TO AN AREA FOR FLOATING POINT
;STORAGE AND R2 SHOULD CONTAIN THE ADDRESS OF A POINTER TO AN AREA FOR INTEGER
;STORAGE. IF R1 OR R2 IS ZERO DEFAULT AREAS WILL BE USED. R3 CONTAINS
;AN IDENTIFICATION NUMBER TO BE PASSED BACK AT BEGINNING OF INTEGER BUF.
;
;A CALL TO SETC SHOULD BE MADE TO INDICATE WHICH ARM IS BEING GATHERED.
;THIS ROUTINE SHOULD BE CALLED JUST PRIOR TO CALLING THE MOVE FOR WHICH
;DATA IS TO BE COLLECTED:
;
; MOV #FBITS,R0
; MOV #FPPTR,R1
; MOV #IPTR ,R2
: MOV #CONTROL,R3
; JSR PC,GATHER
;
;R0,R1 AND R2 PASS ARGUMENTS AND ARE CHHANGED
GATHER:
TST R1 ;IF R1|R2 =0 USE DEFAULT AREAS
BEQ 1$
TST R2
BNE 2$
1$: MOV #FPPTR,R1 ;POINT TO FP POINTER
MOV #GAREA,FPBUF
MOV #GAREA,FPPTR
MOV #INTPTR,R2 ;POINT TO INT POINTER
MOV #GIAREA,INTBUF
MOV #GIAREA,INTPTR
2$: TST R0
BEQ 5$ ;QUIT IF NO CONTROL BITS
MOV R0,GDATA ;SAVE CONTROL BITS
MOV R1,FPSAV ;SAVE POINTER TO FP POINTER
MOV R2,ISAV ;SAVE POINTER TO INTEGER POINTER
MOV R3,IDSAV ;SAVE ID# *K
CLR GTIME ;ZERO TIMES COUNTER
CLR R0 ;ZERO WRIST READINGS
; MOV #BLUARM,C1 ;INDICATE BLUE ARM USING FORCE SYSTEM
5$: RTS PC
;SUBROUTINE TO RESOLVE AND STORE READINGS AS NEEDED
RNS: INC GTIME ;UPDATE COUNTER
MOV GPTR,R1 ;GET DATA POINTER
MOV GDATA,R0 ;GET CONTROL BITS redundant
BPL 1$
RTS PC ;QUIT IF NO RESOLVING NEEDED
;GET FORCE READINGS IN HAND COORDS
1$: MOV R3,-(SP) ;SAVE REG
BIC #177700,R0 ;CLEAR LEFT BYTE OF TORQUE BITS
MOV #STDCAL,R2 ;POINT TO CALIBRATION MATRIX FOR FORCES
GCK: BIT #1,R0 ;STORE THIS READING?
BNE 2$
ADD #4,R2 ;NO, ADVANCE CAL POINTER TO NEXT COL
BR 3$
2$: CLRF AC1 ;COMPUTE THIS FORCE COMPONENT
MOV #EPSF,R4 ;POINT TO GAGE READINGS
MOV #8.,R3 ;8 *'S
1$: LDF (R2),AC0 ;GET CAL ENTRY
MULF (R4)+,AC0 ;* EPSF
ADD #6*4,R2 ;POINT TO NEXT ITEM IN COL.
ADDF AC0,AC1 ;ACCUMULATE
DEC R3
BGT 1$
STF AC1,(R1)+ ;STORE FORCE READING IN DATA AREA
ADD #4-<8.*6*4>,R2 ;POINT TO NEXT COL
3$: ASR R0 ;CHECK NEXT CONTROL BIT
BNE GCK ;CONTINUE UNTIL NO MORE BTS
;CHECK FOR JOINT TORQUE GATHERING
MOV GDATA,R0 ;GET LEFT BYTE
BIC #170077,R0 ;KEEP ONLY TORQUE BITS
ASH #-6,R0 ;RIGHT JUSTIFY BITS
MOV #ERRPTR,R2 ;POINT TO LIST OF TORQUE ERROR POINTERS
BITB #BLUARM,@FCARAY ;WHICH ARM?
BEQ 4$ ;BR IF YELLOW
ADD #14.,R2 ;POINT TO BLUE ARM TERMS
4$: BIT #1,R0 ;COLLECT THIS COMPONENT?
BEQ 5$
LDF @(R2),AC0 ;GET ERROR VALUE
STF AC0,(R1)+ ;AND STORE IT
5$: ADD #2,R2 ;NEXT ERROR TERM
ASR R0
BNE 4$
MOV R1,GPTR ;SAVE DATA POINTER
MOV (SP)+,R3 ;RESTORE
RTS PC
;GATHER DATA AREA
DATA
GCNT: .word 0 ;TRANSIENT GATHER TIMER
GDATA: .WORD 0 ;CONTAINS BITS INDICATING COMPONENTS TO GATHER
GTIME: .WORD 0 ;INDICATES WHICH PASS WE ARE ON
GPTR: .WORD 0 ;POINTS TO NEXT FP STORAGE LOCATION removed by MSM
FPSAV: .WORD 0
GIPTR: .WORD 0 ;POINTS TO INTEGER STORAGE LOCATION
ISAV: .WORD 0
IDSAV: .WORD 0 ;STORAGE FOR ID#
EPS: .BLKW 8. ;STORAGE FOR STRAIN GAGE READINGS
EPSF: .BLKW 2*8. ;FP STORAGE FOR READINGS
;END OF GATHER
;FORCE SENSING ROUTINES DATA AREA
DATA
FZAPS: ;START OF AREA ZEROED BY "INTARM"
FSTAT: .WORD 0 ;FORCE SENSING SYSTEM STATUS WORD, CONTAINS:
;NXTFIN ==1 ;NEXT SERVICING PERIOD WILL BE "FINAL".
;FINAL ==2 ;IN FINAL STAGE OF MOTION, JUST NULLING ERRORS
;RUN ==4 ;LEVEL 6 CODE SCHEDULED TO EXECUTE
BACKG ==20 ;BACKGROUND JOB RUNNING AT LEVEL 5
;FIRST ==40 ;FIRST PASS THRU FORCE CODE
; 177400 ;≠0 MEANS LEVEL 6 CODE LOCKED OUT
FRCNUM: .WORD 0 ;NUMBER OF JOBS PENDING
BCKTME: .WORD 3 ;=0 WHEN TIME TO RUN BACKGROUND JOB
XFPTR: .WORD 0 ;LINK TO JOB BLKS
YFPTR: .WORD 0
ZFPTR: .WORD 0
XMPTR: .WORD 0
YMPTR: .WORD 0
ZMPTR: .WORD 0
BIASF: .FLT2 0.0 ;STORAGE FOR BIAS FORCES
.FLT2 0.0 ;zeroed by SETC, CLRFRC and BISON
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
BIAST: .FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
FZAPE: ;END OF ZERO AREA
FCARAY: C1 ;PTR TO CURRENT C ARRAY AND ARM DATA
C1: 0 ;YELARM/BLUARM AND TABLE/HAND BITS
.BLKW 12.*2 ;FORCE TRANS
C2
C2: 0
.BLKW 12.*2
C1
NEXTC== C2-C1-2 ;REL. PTR TO RING LINKAGE WORD
FJARAY: J1 ;PTR TO CURRENT JACOBIAN ARRAY
J1: .BLKW 42.*2 ;JACOBIAN ARRAY + SUM OF SQUARES
J2
J2: .BLKW 42.*2
J1
NEXTJ== J2-J1-2 ;REL. PTR TO RING LINKAGE WORD
IMAT: .FLT2 1.0,0.0,0.0 ;IDENTITY MATRIX
.FLT2 0.0,1.0,0.0
.FLT2 0.0,0.0,1.0
.FLT2 0.0,0.0,0.0
TTRAN: .BLKW 12.*2 ;TEMPORARY TRANSFORM STORAGE
FINUSE: .BLKW 1 ;INUSE
FRCPTR: FRCJOB ;PTR TO CHAIN OF FREE BLKS IN JOB LIST
FRCJOB: .BLKW 7*FRCBLK;JOB BLOCKS CONTAIN:
; WRD 1: LINK TO NEXT BLOCK
; WRD 2,3: F.P. MAGNITUDE
; WRD 4: FORCE OPTION BITS
; WRD 5: COUNTER FOR # OF TIMES TEST OK
; WRD 6,7: STRT ADDR OF JOB&PDB
FRCPD5: PDBLK 5,100,FP;LEVEL 5 PDB
;END OF "FORCE" ROUTINES
;FFORCE - reads finger force values
;SHOULD BE CALLED WITH R0 CONTAINING NUMBER (SEE BELOW) INDIACTING WHICH
;FINGER TO READ
;
;FORCE VALUE IS RETURNED IN AC0
;RAW READING IS RETURNED IN R0
;TWO TIMES FINGER NUMBER IS RETURNED IN R1
;
;ROUTINE SHOULD ONLY BE CALLED BY LEVEL 6 PROCESS TO PREVENT ADC CONFLICTS
;
; FINGER NUMBER DEFINITIONS:
; BLUE ARM, FINGER #0 0
; BLUE ARM, FINGER #1 1
; BLUE HAND REFERENCE 2
; BLUE HAND REFERENCE 3
; YELLOW ARM, FINGER #0 4 BINARY - RETURNS 500.0 IF TOUCHING
; YELLOW ARM, FINGER #1 5 BINARY - ELSE RETURNS 0.0
; OTHER TO BE ADDED HERE
CODE
FFORCE: CMP #3,R0 ;CHECK TO SEE IF ON YELLOW INTERFACE
BLT 3$ ;BR IF SENSOR # > 3
MOVB FCHAN(R0),ADCCHN ;START CONVERSION IN BLUE INTERFACE
1$: TSTB ADCSR ;CONVERSION DONE?
BPL 1$ ;BR IF NOT READY
MOV ADCVAL,R1 ;GET VALUE
2$: MOV R1,-(SP) ;SAVE RAW READING
ASL R0 ;GET WORD POINTER
SUB FZERO(R0),R1 ;SUBTRACT ZERO READING
LDCIF R1,AC0 ;CHANGE TO F.P.
MOV R0,R1 ;SAVE WORD POINTER
ASL R0 ;GET F.P. POINTER
MULF FSCALE(R0),AC0 ;CONVERT TO OZ.
MOV (SP)+,R0 ;RETURN RAW READING
RTS PC
3$: CLR R1 ;ASSUME SENSOR OFF
MOV #43,DR11S ;PREPARE INTERFACE TO READ SENSOR BIT
CMP #4,R0 ;FINGER 0 OR 1?
BNE 4$
BIT #1,DR11I ;FINGER 0
BR 5$
4$: BIT #2,DR11I ;FINGER 1
5$: BEQ 2$
INC R1 ;SAY SENSOR ON
BR 2$
; MOV #42,DR11S ;SET YELLOW INTERFACE
; MOVB FCHAN(R0),DR11O ;START CONVERSION IN YEL INTERFACE
; TSTB DR11S
; BPL .-4 ;WAIT
; MOV DR11I,R1 ;GET VALUE
; SUB #3777,R1 ;OFFSET SO 0=0
; BR 2$
;DATA FOR FINGER FORCE ROUTINE
DATA
FCHAN: .BYTE 1 ;FINGER 1 ADCCHN 34. if ffings
.BYTE 0 ;FINGER 2 ADCCHN 35. if ffings
.BYTE 42 ;FINGER REF CHANNEL
.BYTE 43 ;FINGER REF CHANNEL
.BYTE 0 ;BEGIN YELLOW FORCE SENSOR CHANNELS HERE
.BYTE 0
FZERO: .WORD 0 ;ZERO READINGS ON FINGERS
.WORD 0
.WORD 0
.WORD 0
.WORD 0
.WORD 0
FSCALE: .FLT2 1.0 ;SCALE FACTORS FOR CONVERSION TO OZ
.FLT2 1.0
.FLT2 1.0
.FLT2 1.0
.FLT2 500.0 ;YELLOW TOUCH SENSORS
.FLT2 500.0
;A simple test showed the scale factor for the force-sensing fingers is
;about 1.7 ADC counts per gram. -RV
;END OF FFORCE ROUTINE
;SETSTF - initializes stiffness system
;R0 SHOULD POINT TO LIST OF 6 FP STIFFNESS VALUES: Kx,Ky,Kz,Gx,Gy,Gz
;R1 SHOULD POINT TO A RELATIVE TRANSFORM BETWEEN THE HAND FRAME OF
;REFERENCE AND THE FRAME IN WHICH THE STIFFNESS VALUES ARE DEFINED
;
;STFDAT IS INITALIZED
;R0 GARBAGED, ALL OTHER REGS PRESEVED
CODE
SETSTF: MOV R1,-(SP)
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;PUT REL XFORM TO STIFFNESS FRAME IN C66
MOV R1,RELX ;SAVE POINTER TO REL. XFORM
mov #c66,r2
mov #3,r3 ;3 cols in rel xform
3$: mov #3,r4 ;3 elements in each col
2$: ldf (r1)+,ac0 ;get element of orientation
stf ac0,(r2)+ ;store in upper left 3x3
stf ac0,<20.*4>(r2) ; " lower right
sob r4,2$
add #<3*4>,r2
sob r3,3$
MOV #C66,R2 ;transfer displacements
LDF (R1)+,AC0
STF AC0,<31.*4>(R2) ;X
NEGF AC0
STF AC0,<26.*4>(R2) ;-X
LDF (R1)+,AC0
STF AC0,<20.*4>(R2) ;Y
NEGF AC0
STF AC0,<30.*4>(R2) ;-Y
LDF (R1),AC0
STF AC0,<24.*4>(R2) ;Z
NEGF AC0
STF AC0,<19.*4>(R2) ;-Z
MOV #KC,R1 ;STORE CARTESIAN STIFFNESS VALUES HERE
MOV #6,R2
1$: LDF (R0)+,AC0
CFCC
BLT .+4
NEGF AC0 ;MAKE SURE ONLY NEG. STIFFNESS VALUES USED
STF AC0,(R1)+
SOB R2,1$ ;6 FP #'S TO TRANSFER
MOV #KC,R0 ;COMPUTE CORRESPONDING DIAGONAL DAMPING TERMS
MOV #KVC,R1 ; AND STORE HERE
LDF KVTRAN,AC1 ;TRANSLATION PROPORTIONALLITY CONSTANT
LDF (R0)+,AC0
MULF AC1,AC0
STF AC0,(R1)+
LDF (R0)+,AC0
MULF AC1,AC0
STF AC0,(R1)+
LDF (R0)+,AC0
MULF AC1,AC0
STF AC0,(R1)+
LDF KVROT,AC1 ;ROTATION PROPORTIONALLITY CONSTANT
LDF (R0)+,AC0
MULF AC1,AC0
STF AC0,(R1)+
LDF (R0)+,AC0
MULF AC1,AC0
STF AC0,(R1)+
LDF (R0)+,AC0
MULF AC1,AC0
STF AC0,(R1)+
MOV #BLUARM,C1 ;INDICATE BLUE ARM USING FORCE SYSTEM
;INITALIZE CI AND CII TERMS
MOV #BTH,R0 ;ELSE COMPUTE CURRENT DYNAMIC COEF
MOV #BCI,R1 ;UPDATE SERVO DATA BLOCKS
MOV #BLUARM,R2 ;INDICATE BLUE ARM
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
;COMPUTE JACOBIAN MATRIX, STIFFNESS MATRIX AND JTCAL MATRIX
GETJAC: MOV #IMAT,R0 ;POINTER TO FORCE TRANS
MOV #THPTR,R1 ;POINT TO LIST OF ALL JOINT ANGLES
MOV #BLUARM+FHAND,R2;INDICATE BLUE ARM IN HAND COORDS
MOV #JC,R3 ;POINT TO RESULT MATRIX
JSR PC,JACOB ;COMPUTE JACOBIAN
JSR PC,GJTCAL ;COMPUTE JOINT TORQUE CALIBRATION MAT.
MOV RELX,R1 ;POINT TO REL TRANSFORM
JSR PC,GETKTH ;COMPUTE STIFFNESS MATRIX
MOV #RUN,STFDAT ;INDICATE NEXT MOVE IS A KMOVE
bis #run,fstat ;get frclv6 running so FBACK is run periodically
MOV (SP)+,R5
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC
;END OF SETSTF
; KSRVO - services all joints once per tick
;SERVOS EACH JOINT IN DEVICE BLOCK POINTED TO BY DAT
ksrvo: mov devpnt,dat ;get pointer to device block
TST (DAT) ;CHECK if ERROR or force sensing termination
BLE KQUIT
TST BPANIC ;PANIC ?
BNE KQUIT
CMP (DAT)+,(DAT)+ ;POINT TO FIRST SERVO POINTER
MOV DAT,KSRV ;SAVE SERVO POINTER
; bit #moving,@(dat) ;have we released brakes yet? **K?
; bne 7$
; mov #6,r2
; mov dat,r1
;6$: mov (r1)+,dat
; jsr pc,brkrel ;no release brake
; bis #moving,(dat) ;indicate we are moving now
; sob r2,6$
; br ksvdn ;dont do anything else this pass
7$: JSR PC,REPS ;READ GAGES FOR THIS PASS
LDCIF KPTIME,AC1 ;CONVERT THE PRESENT TIME TO FLOATING POINT
INC KPTIME ;INCREMENT FOR NEXT TIME
STF AC1,KFPTIM ;SAVE FOR EVLP
CMPF KFTTIM,AC1
CFCC
BGT EVLP ;DONT QUIT IF NOT THRU SEGMENT YET
;CHECK FOR NEW SEG HERE
MOV KSRV,R2
MOV (R2)+,DAT
MOV DATPT(DAT),R0 ;PTR TO START OF POLY. DATA LIST
MOV RNCODE(R0),R1 ;CHECK IF POSSIBLE EVENT TO SIGNAL
BEQ 3$
TST 2(R1) ;GET EVENT #
BEQ 3$ ;ZERO IF ALREADY SIGNALED
MOV R0,-(SP) ;SAVE REGS SO KERNEL WON'T CLOBBER 'EM
MOV R1,-(SP)
MOV R2,-(SP)
MOV DAT,-(SP)
EVSIG 2(R1)
MOV (SP)+,DAT ;RESTORE REGS.
MOV (SP)+,R2
MOV (SP)+,R1
MOV (SP)+,R0
CLR 2(R1) ;ZERO TO INDICATE SIGNALED
3$: MOV (R0),R3 ;YES,SWITCH TO A NEW SEGMENT
ADD R3,R0 ;MOVE PTR
TST (R0) ;FINAL SEGMENT?
beq kquit ;br if so
1$: MOV SEGTME(R0),R1 ;GET NEW SEGTIME
ASH #-4,R1 ;CONVERT TO SERVO PERIODS
LDCIF R1,AC0
STF AC0,KFTTIM ;STORE NEW SEGTIM
2$: MOV R0,DATPT(DAT) ;RESET ALL JOINT POLY POINTERS
ADD R3,POLY(DAT)
MOV (R2)+,DAT
BNE 2$
CLR KPTIME ;BEGINNING OF A NEW SEG.
CLRF KFPTIM
CLRF AC1 ;ZERO TIME
;SERVOING LOOP
EVLP: MOV @KSRV,DAT ;GET SERVO POINTER
BEQ KSVDN ;IF ZERO THEN DONE FOR THIS TICK
ADD #2,KSRV ;POINT TO NEXT SERVO POINTER for next pass
; bit #moving,(dat) ;have we released brakes yet?
; bne 1$ ; ** bug - should not need it every time **
jsr pc,brkrel ;no - release brake
bis #moving,(dat) ;indicate we are moving now
1$: LDF KFPTIM,AC1
MOV POLY(DAT),CC ;GET ABSOLUTE ADDRESS OF POSITION POLYNOMIAL
DIVF KFTTIM,AC1 ;GET THE FRACTIONAL TIME INTO THIS SEGMENT
LDF (CC),AC0 ;EVALUATE THE POSITION POLYNOMIAL, GET A5
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A4
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A3
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A2
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A1
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A0, NOW HAVE POLYNOMIAL JOINT ANGLE
STF AC0,AC1
SUBF THP(DAT),AC1 ;COMPUTE PREDICTED ANGULAR CHANGE
DIVF KELTIM,AC1 ;DIVIDE BY ELAPSED TIME
STF AC1,THDP(DAT) ;SAVE VEL.
STF AC0,THP(DAT) ;SAVE POSITION
JSR PC,SS2 ;SERVO JOINT POINTED TO BY DAT
BR EVLP ;DO FOR ALL JOINTS IN DEVICE BLOCK
KQUIT: mov devpnt,r1 ;POINTER TO DEVICE BLOCK
MOVB (R1),R2 ;GET # ATTACHED
CMP (R1)+,(R1)+ ;point to first servo pointer
1$: MOV (R1)+,DAT ;point to servo data block
jsr pc,motstp ;shut motor down and set brakes
bic #run+moving,(dat) ;we are done moving and running
sob r2,1$
clrb @devpnt ;indicate all servos done so event will be signaled
clr stfdat ;no longer stiffness servoing
KSVDN: rts pc ;done
; SS2 - computes trajectory deviation and determines force to apply
;EXECUTION TIME = .95 msec
;DAT POINTS TO SERVO TO SERVICE
CODE
SS2: JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
SUBF THP(DAT),AC0 ;COMPUTE DELTA THETA (POS ERROR)
MULF TOTQE(DAT),AC0 ;CONVERT TO RAD. OR IN.
STF AC0,POSERR(DAT) ;AND SAVE IT
SUBF THDP(DAT),AC1 ;FIND VEL. ERROR IN MOVING FRAME
STF AC1,VELERR(DAT) ;AND SAVE IT
;DETERMINE JOINT #
CLR R0
MOVB SRVNUM(DAT),R0
SUB #7,R0 ;COMPUTE JOINT NUMBER
MOV R0,-(SP) ;AND SAVE
;COMPUTE COMMANDED TORQUE FOR THIS JOINT BASED ON STIFFNESS AND DAMPING MATRICES
JSR PC,TCI ;COMPUTE TC FOR THIS JOINT
kn1: STF AC0,TC ;SAVE
;DETERMINE TORQUE ERROR, U, ON THIS JOINT
MOV (SP),R0 ;GET JOINT #
JSR PC,TSI ;GET TORQUE ON JOINT #R0
cmp #6,(sp) ;fix for inverted joint 6 torque
beq kn2
NEGF AC0
kn2: ADDF TC,AC0 ;TORQUE ERROR U=TC-TS
;COMPUTE TORQUE COMPENSATON
MOV (SP)+,R0 ;GET SERVO # AGAIN
JSR PC,SS1 ;U→AC0 AND SERVO # →R0 →→ Y→ACO
;ADD VELOCITY DAMPING AND GRAVITY COMPENSATON
kn3: LDF KKV(DAT),AC1 ;COMPUTE VELOCITY FEEDBACK GAIN
MULF CII(DAT),AC1 ;FOR STABILITY W/ INERTIAS
DIVF KKK(DAT),AC1 ;FOR STABILITY W/ DIFFERENT STIFFNESS *fix me
ldf thd(dat),ac2 ;GET ABSOULTE VELOTITY
CFCC ;NEED TO CHECK ABSOUTE VELOCITY FOR FRICTION
BGE 1$ ; COMPENSATION
SUBF V0(DAT),AC0 ;NEG. VEL. => NEG. FRICTION COMPENSTAION
BR 2$
1$: ADDF V0(DAT),AC0 ;POS. VEL. => POS. FRICTION COMPENSATION
2$: LDF VELERR(DAT),AC2 ;GET VEL. ERROR IN MOVING FRAME
MULF AC2,AC1 ;* GAIN
ADDF AC1,AC0 ;ADD VELOCITY FEEDBACK TO DRIVE VALUE
ADDF TC,AC0 ;FEED FORWARD COMMANDED TORQUE
ADDF CI(DAT),AC0 ;GRAVITY COMPENSATION
kn4: STF AC0,ERRTQE(DAT) ;SAVE FOR GATHER ROUTINES
JSR PC,MOTDRV ;DO IT
RTS PC
; SS1 - computes force compensation
;CALLED WITH AC0←U
; WHERE U=Tc-Ts
;RETURNS AC0←Y
; WHERE Y=(S+A)/(S+B) + Q
; Q= QOLD + Ti*SGN(U) FOR |U|>Tdbd
; = QOLD FOR |U|≥Tdbd
;COMPUTE Y1 (ZERO ORDER HOLD APPROX OF LEAD-LAG FILTER)
SS1: STF AC0,AC2 ;SAVE U FOR LATER
LDF ALPHA,AC1
MULF UOLD(DAT),AC1
SUBF AC1,AC0
LDF BETA,AC1
MULF YOLD(DAT),AC1 ;BETA*Yold
ADDF AC1,AC0 ;Y1=U-ALPHA*Uold+BETA*yold
STF AC0,YOLD(DAT) ;SAVE FOR NEXT TIME
STF AC2,UOLD(DAT) ;SAVE U FOR NEXT PASS
MULF KKF(DAT),AC0 ;GAIN
; ___
;COMPUTE Y2 (INTEGRAL W/ LIMITER AND DEADBAND)→ __| * 1/S
; ___|
LDF KKI(DAT),AC1 ;ASSUME NOT IN DEADBAND
TSTF AC2 ;WAS U POS OR NEG?
CFCC
BPL 2$
NEGF AC1 ;IF ERROR <0 NEGATE TI
2$: ABSF AC2
CMPF TDBD,AC2 ;TDBD<|U|?
CFCC
BLT 3$ ;BR IF TDBD < |U|
CLRF AC1 ;INSIDE DEADBAND SO 0 OUTPUT
3$: ADDF ERRINT(DAT),AC1 ;INTEGRATE FORCE ERROR
kn5: STF AC1,ERRINT(DAT) ;SAVE INTEGRAL
ADDF AC1,AC0 ;Y= KKF*Y1 + Y2
RTS PC
; REPS - reads raw force sensor data
;READS FORCE SENSOR ON BLUARM AND SUBTRACTS BASE READINGS FROM IT.
;LEAVES FLOATING POINT CORRECTED READINGS AT EPSF.
;PRESERVES DAT AND ALTERS R0-R4,AC0
;RUNTIME IS .5 MSEC
REPS: MOV DAT,-(SP) ;SAVE DAT
MOV #EPSF,DAT ;WHERE TO PUT FLOATING PT EPS
MOVB #30,ADCCHN ;START FIRST CONVERSION
MOV #BASRDG,R0 ;POINT TO BASE READINGS
MOV #8.,R1 ;EIGHT SENSORS TO READ
MOV #31,R3 ;NEXT CHANNEL TO READ
MOV #EPS,R4 ;INTEGER READING ARRAY
1$: TSTB ADCSR
BPL 1$
MOV ADCVAL,R2 ;GET READING
MOVB R3,ADCCHN ;START NEXT READING
TSTB (R3)+ ;POINT TO NEXT CHANNEL
SUB (R0)+,R2 ;SUBTRACT BASE READING
;FILTERING ON CC GOES HERE
MOV R2,(R4)+ ;ADD CHANGE TO EPS ARRAY
LDCIF R2,AC0
STF AC0,(DAT)+ ;STORE FLOATING POINT EPS
DEC R1
BGT 1$
MOV (SP)+,DAT ;RESTORE DAT
RTS PC
; TCI, TSI - computes commanded and sensed torque in I-th joint
;TCI PROCEDURE TO COMPUTE COMMANDED TORQUE FOR I-TH JOINT BASED ON STIFFNESS
;AND DAMPING MATRICES KTH AND KVTH.
;R0 SHOULD CONTAIN NUMBER OF JOINT TO COMPUTE TORQUE FOR.
;R0-R2 AND AC1 DESTROYED. AC0 CONTAINS RESULT.
TCI: DEC R0
BGE 1$
BPT ;BAD JOINT #
1$: MOV R0,-(SP) ;SAVE JOINT #-1
CLRF AC0 ;CLEAR TORQUE ACCUMULATOR
MUL #24.,R0 ;COMPUTE OFFSET INTO KTH AND KVTH (ANS IN R1)
MOV R1,-(SP) ;SAVE IT
ADD #KTH,R1 ;POINT TO PROPER ROW OF KTH
MOV #PERPTR,R0 ;POINT TO LIST OF POINTERS TO POS. ERRS.
JSR PC,DOTTCI ;ADD RESOLVED STIFFESS TORQUE INTO AC0
MOV (SP)+,R1 ;GET MATRIX OFFSET
ADD #KVTH,R1 ;POINT TO PROPER ROW OF KVTH
MOV #VERPTR,R0 ;POINT TO LIST OF POINTERS TO VEL. ERRS.
JSR PC,DOTTCI ;ADD RESOLVED DAMPING TORQUE INTO AC0
MOV (SP)+,R0 ;GET JOINT #-1 AGAIN
MUL #4.,R0 ;COMPUTE OFFSET FOR BIAS TORQUE
ADDF BIAST(R1),AC0 ;ADD BIAS IF ANY
RTS PC
DOTTCI: MOV #6,R2 ;DO 6 PRODUCTS
2$: LDF @(R0)+,AC1 ;POSERR OR VELERR
MULF (R1)+,AC1 ;*K(I,J)
ADDF AC1,AC0 ;ACCUMULATE
SOB R2,2$ ;6 TIMES
RTS PC
;TSI COMPUTES SENSED TORQUE ON I-TH JOINT OF BLUE ARM
;R0 SHOULD CONTAIN NUMBER OF JOINT FOR WHICH TORQUE IS DESIRED
;RESULT IS RETURNED IN AC0 (OZ-IN)
;DESTROYS R0-R2,AC0,AC1
TSI: DEC R0
BGE 1$
BPT
1$: ASH #5,R0 ;COMPUTE OFFSET IN JTCAL
ADD #JTCAL,R0 ;POINT TO PROPER ROW OF JTCAL
MOV #8.,R1
CLRF AC0
MOV #EPSF,R2
2$: LDF (R0)+,AC1
MULF (R2)+,AC1
ADDF AC1,AC0
SOB R1,2$
RTS PC
; GJTCAL, FINKTH - computes torque transform and stiffness matrices
;ROUTINE COMPUTES MATRIX THAT TRANSFORMS FORCE SENSOR READINGS TO JOINT TORQUES
;DESTROYS R0-R5,AC0,AC1
;ASSUMES JC HAS BEEN COMPUTED ALREADY
CODE
GJTCAL: MOV #JC,R0 ;points to jacobian in hand coords.
MOV #STDCAL,R1
MOV #JTCAL,R2
MOV #6,R5 ;6 ROWS TO CALCULATE
1$: MOV #8.,R4 ;8 ELEMENTS IN EACH ROW
2$: MOV #6,R3 ;6 PRODUCTS TO MAKE AN ELEMENT
CLRF AC0
3$: LDF (R0)+,AC1 ;GET JT ELEMENT
MULF (R1)+,AC1 ;* CAL ELEMENT
ADDF AC1,AC0 ;ACCUMULATE
SOB R3,3$
STF AC0,(R2)+ ;STORE ELEMENT OF JTCAL
SUB #24.,R0 ;BACK TO PREVIOUS ROW OF JT
SOB R4,2$
ADD #24.,R0 ;NEXT ROW OF JT
SUB #192.,R1 ;BACK TO BEGINNING OF CAL
SOB R5,1$
RTS PC
;ROUTINE TO COMPUTE JOINT STIFFNESS MATRIX, KTH AND DAMPING MATRIX KVTH
;R1 SHOULD POINT TO TRANS BETWEEN HAND AND STIFFNESS FRAME
;
; CLOBBERS R0-R4
GETKTH: MOV R5,-(SP) ;SAVE R5
;COMPUTE JACOBIAN TO STIFFNESS FRAME
MOV #JPRIME,R0
MOV #C66,R1
MOV #JC,R2
MOV #6,R5 ;6 COLS TO FILL IN RESULT
JSR PC,MATM66
;COMPUTE KTH←J'T*(KC*J')
MOV #KTH,-(SP) ;POINT TO ANSWER STORAGE
MOV #KC,-(SP) ;POINT TO DIAGONAL CARTESIAN MATRIX ELEMENTS
JSR PC,MAT3 ;COMPUTE MATRIX PRODUCTS
CMP (SP)+,(SP)+ ;POP 2 ARGUMENTS OFF STACK
;COMPUTE KVTH←J'T*(KVC*J')
MOV #KVTH,-(SP) ;POINT TO ANSWER STORAGE
MOV #KVC,-(SP) ;POINT TO ANSWER STORAGE
JSR PC,MAT3 ;COMPUTE MATRIX PRODUCTS
CMP (SP)+,(SP)+ ;POP 2 ARGUMENTS OFF STACK
MOV (SP)+,R5 ;RESTORE R5
RTS PC
;INTERNAL ROUTINE TO RESOLVE CARTESIAN MATRIX (DIAGIONAL FORM) INTO JOINT SPACE
MAT3: MOV #JPRIME,R1 ;POINT TO NEW JACOBIAN AND COMPUTE INTERMEDIATE PRODUCT (KC*J')
MOV #KCJ,R2 ;POINT TO STORAGE FOR KC*J MATRIX (INTERMED PROD)
MOV #6,R4 ;6 COLUMNS
2$: MOV 2(SP),R0 ;POINT TO CARTESIAN STIFFNESS MATRIX (DIAG ELEMENTS)
;2$: MOV #KC,R0
MOV #6,R3 ;6 ENTRIES/COLUMN
1$: LDF (R1)+,AC0 ;GET ENTRY
MULF (R0)+,AC0 ;* KC
STF AC0,(R2)+ ;STORE RESULT
SOB R3,1$
SOB R4,2$
;COMPUTE FINAL PRODUCT
FPROD: MOV #JPRIME,R0
MOV 4(SP),R1 ;POINT TO MATRIX TO STORE KTH (COLUMN MAJOR)
; MOV #KTH,R1 ;POINT TO MATRIX TO STORE KTH (COLUMN MAJOR)
MOV #KCJ,KCJCOL ;POINT TO FIRST COLUMN OF KCJ (INTERMEDIATE PROD)
MOV #6,R5 ;DO FOR 6 COLUMNS
3$: MOV #JPRIME,R0 ;POINT TO J'T (JPRIME TRANSPOSE)
MOV #6,R3 ;COMPUTE SIX ENTRIES FOR THIS COLUMN
1$: MOV KCJCOL,R2 ;POINT TO COLUMN OF KCJ
CLRF AC2 ;CLEAR ACCUMULATOR
MOV #6,R4 ;EACH KTH ENTRY COMES FROM 6 PRODUCTS
2$: LDF (R2)+,AC0 ;GET ITEM FROM COLUMN OF KCJ
LDF (R0)+,AC1 ;GET CORRESPONDING ROW ITEM FROM JT
MULF AC0,AC1
ADDF AC1,AC2 ;ACCUMULATE
SOB R4,2$ ;DO 6 OF 'EM
STF AC2,(R1)+ ;SAVE ENTRY IN KTH OR KVTH
SOB R3,1$ ;FOR ALL 6 ROWS OF JT
ADD #24.,KCJCOL ;POINT TO NEXT COLUMN OF KCJ
SOB R5,3$ ;AND DO AGAIN
RTS PC
;MATM66 MULTIPLIES 6*6 BY 6*1 VECTORS OR 6*6 MATRICES
;PERFORMS (R0)←(R1)*(R2)
;R0-R5 DESTROYED
;R5 SHOULD CONTAIN 1 IF FOR MATRIX VECTOR PRODUCT
;R5 SHOULD CONTAIN 6 IF FOR MATRIX MATRIX PRODUCT
;(R0) SHOULD NOT BE SAME AS (R1) OR (R2)
;2.5 MSEC EXECUTION TIME
MATM66: MOV R1,-(SP) ;SAVE ROW POINTER
11$: MOV (SP),R1 ;FIRST ROW AGAIN
MOV #6,R4 ;6 ROWS TO DO
BR 3$
6$: SUB #116.,R1 ;NEXT ROW
SUB #24.,R2 ;SAME COL
3$: MOV #6,R3 ;DOT ROW AND COL TOGETHER
CLRF AC0 ;CLEAR ACCUMULATOR
BR 2$
1$: ADD #24.,R1 ;NEXT ITEM IN ROW
2$: LDF (R2)+,AC1 ;GET NEXT ITEM IN COL.
CFCC
BEQ 27$ ;SKIP ZEROS IN SPARSE MATRIX
MULF (R1),AC1 ;* NEXT ITEM IN COL
ADDF AC1,AC0
27$: SOB R3,1$ ;6 ITEMS TO DO
STF AC0,(R0)+ ;STORE (I,J) RESULT
SOB R4,6$ ;6 ROWS TO DO
SOB R5,11$ ;6 COLS TO FILL
TST (SP)+ ;CLEAR STACK
RTS PC
;GKKK GETS PRINCIPAL STIFFNESS COEFFICENT FOR I'TH JOINT (NOT CURRENTLY USED)
;R0 SHOULD CONTAIN NUMBER OF JOINT FOR WHICH KKK IS DESIRED
GKKK: DEC R0
MUL #28.,R0 ;POINT TO I,I ELEMENT OF MATRIX
LDF KTH(R0),AC2 ;GET IT
RTS PC
; DATA for stiffness servo system
DATA
STFDAT: .WORD 0 ;STATUS OF STIFFNESS SYSTEM
KC: .FLT2 -50.0 ;CARTESIAN STIFFNESS MATRIX (DIAGONAL)
.FLT2 -50.0
.FLT2 -50.0
.FLT2 -10000.0
.FLT2 -10000.0
.FLT2 -500.0
KVC: .FLT2 0.0 ;CARTESIAN DAMPING MATRIX
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
KVTRAN: .FLT2 0.5 ;TRANSLATION TIME CONSTANT KVCi←KCi/KVTRAN
KVROT: .FLT2 0.0 ;ROTATION TIME CONSTANT KVCi←KCi/KVTRAN
BIASJA: JC ;POINTER TO JACOBIAN TO BIAS FORCE FRAME
KSRV: .WORD 0 ;POINTS TO CURRENT SERVO POINTER IN DEV. BLK.
KFTTIM: .FLT2 40.0 ;TOTAL SEGMENT TIME IN TICKS→2/3 SEC.
KPTIME: .WORD 0 ;PRESENT TIME INTO SEGEMET IN TICKS
KFPTIM: .FLT2 0.0 ;F.P. PTIME
KELTIM: .FLT2 16.0 ;ELAPSED TIME BETWEEN SERVOING IN msec
TC: .FLT2 0.0
ALPHA: .FLT2 0.78
BETA: .FLT2 0.07
TDBD: .FLT2 30.0 ;1 OZ AT 30 IN. DEADBAND
;FILTER PARAMETERS
AA: .FLT2 31.4 ;5*6.283 = 5 HZ ZERO
BB: .FLT2 157.0 ;25.0*6.283 = 25 HZ POLE
SPT: .FLT2 0.016 ;SEC/TIC (CURRENTLY SEC/SERVO PERIOD)
;DATA STORAGE FOR STIFFNESS SYSTEM MATRICES
JTCAL: .BLKW 96. ;STORAGE FOR JOINT TORQUE CALIBRATION MAT.
JC: .FLT2 1.0,0.0,0.0,0.0,0.0,0.0 ;6x6 FLOATING PT. JACOBIAN MATRIX
.FLT2 0.0,1.0,0.0,0.0,0.0,0.0
.FLT2 0.0,0.0,1.0,0.0,0.0,0.0
.FLT2 0.0,0.0,0.0,1.0,0.0,0.0
.FLT2 0.0,0.0,0.0,0.0,1.0,0.0
.FLT2 0.0,0.0,0.0,0.0,0.0,1.0
KCJCOL: KCJ ;POINTER TO COLUMN OF KCJ
KCJ: .BLKW 36.*2 ;STORAGE FOR INTERMEDIATE PRODUCT
KTH: .BLKW 36.*2 ;STORAGE FOR STIFFNESS MATRIX (COL MAJ)
KVTH: .BLKW 36.*2 ;STORAGE FOR DAMPING MATRIX (COL MAJ)
JPRIME: .BLKW 36.*2 ;STORAGE FOR JACOBIAN TO STIFFNESS FRAME,C
RELX: .WORD 0 ;POINTER TO RELATIVE XFORM FROM HAND TO STIFFNESS
;CONTROLLED FRAME
; .FLT2 1.0,2.0,3.0 ;mem test
C66: .FLT2 1.0,0.0,0.0,0.0,0.0,0.0 ;6*6 REL XFORM FROM HAND TO C
.FLT2 0.0,1.0,0.0,0.0,0.0,0.0
.FLT2 0.0,0.0,1.0,0.0,0.0,0.0
.FLT2 0.0,0.0,0.0,1.0,0.0,0.0
.FLT2 0.0,0.0,0.0,0.0,1.0,0.0
.FLT2 0.0,0.0,0.0,0.0,0.0,1.0
;END OF STIFFNESS DATA AREA
;OPERATE - setsup screwdriver or vise operation
;
;THIS ROUTINE WILL INITIATE THE VISE OR SCREWDRIVER OPERATION. R0 MUST
;POINT TO A COEFFICENT BLOCK AS DESCRIBED BELOW. R1 POINTS TO STORAGE FOR
;DEVICE BLOCK (5 WORDS). THE VISE CAN BE MADE TO STOP ON CONTACT BY SETTING
;THE NNUL BIT IN THE MODBITS OF THE COEFF. LIST. THE CONTACT FORCE WILL DEPEND
;UPON THE STOPWAIT TIME GIVEN. IF THE SCREW DRIVER IS GIVEN BOTH TORQUE AND
;VELOCITY COMMANDS THE TORQUE COMMAND WILL BE FOLLOWED. TORQUE COMMAND MUST
;BE ZERO FOR VELOCITY SERVOING TO OCCUR. THE SCREWDRIVER VARIABLE TH WILL
;CONTAIN THE NUMBER OF DEGREES TURNED SINCE STARTING TO TURN AND IS RESET AT
;THE BEGINNING OF SCREWDRIVER OPERATION.
;
;THE ROUTINE RETURNS AFTER THE DEVICE IS RUN WITH AN ERROR CODE IN R0
;IF THERE WERE ANY RUN ERRORS.
;
;SAMPLE CALL:
; MOV #COFLST,R0
; MOV #DEVICE,R1
; JSR PC,OPERATE
; TST R0
; BMI ERROR
;
;THE COEFFICENT LIST IS AS FOLLOWS:
;
;(R0)→ 1 ;SERVO BITS: 2 FOR VISE, 1 FOR DRIVER
; 0 ;SERVO BITS
; 0 ;MODBITS 1→no nulling, 0→exactly
; 0 ;WOBPTR
; 0 ;RELSEG PTR
; 250 ;TIME IN TICKS/20
; 0.0 ;TORQUE IN OZ-IN FOR DRIVER, STOPWAIT TIME IN MSEC FOR VISE (FP #)
; 100.0 ;VELOCITY IN DEG/MSEC FOR DRIVER, POSITION GOAL (IN) FOR VISE (FP #)
;CHANGE WHERE TO PASS BACK TH FOR DRIVER AND VISE
CODE
OPERATE:MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE DEV POINTER
MOV R1,OPRDEV ;
MOV R0,OPRBLK ;SAVE POINTER TO COEF. DATA LIST IN INFO BLK
MOV #OPRBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC OPINIT ;BRANCH IF NO ATTACH ERRORS
;SET UP RUN PARAMETERS
OPINIT: MOV (SP),R0 ;POINT TO DEV BLK
CMP (R0)+,(R0)+
MOV (R0),DAT ;GET SERVO POINTER
MOV OPRBLK,R1 ;POINT TO OPERATE COEFF LIST
ADD #12,R1 ;POINT TO OPERATE TIME
MOV (R1)+,R3
CLR R2
DIV #25.,R2 ;CHANGE TO 40 MSEC TICKS
; DIV #20.,R2 ;CHANGE TO 40 MSEC TICKS
MOV R2,PTIME(DAT) ;SET RUNTIME
LDF (R1)+,AC0 ;SET FORCE LEVEL OR STOP WAIT TIME
STF AC0,FLEVEL(DAT)
LDF (R1)+,AC0 ;SET VELOCITY OR POSITION
bit #SCRE,MECHSM(DAT)
BEQ 1$ ;BR IF NOT SCREWDRIVER
STF AC0,THDP(DAT) ;SET VELOCITY GOAL
CLRF TH(DAT) ;0 DEGREES TURNED
CLRF THD(DAT) ;0 INITIAL VELOCITY
CLRB DACINF(DAT) ;ZERO INITIAL OUTPUT
BR DVSCH
1$: bit #VIS,MECHSM(DAT)
BEQ 3$ ;BR IF NOT VISE
MOV #NOSOLU,R0
CMPF USTOP(DAT),AC0 ;CHECK IF IN RANGE
CFCC
BLE OPRDNE
CMPF LSTOP(DAT),AC0
CFCC
BGE OPRDNE
STF AC0,THP(DAT) ;SET POSITION GOAL
BR DVSCH
3$: MOV #WRGNUM,R0 ;MUST NOT BE VISE|SCREWDRIVER
BR OPRDNE
;RUN THE DEVICE REQUESTED
DVSCH: EVMAK ;THIS EVENT SIGNALS SERVOS DONE
MOV 2(SP),R1 ;GET ADDRESS OF DEVICE BLOCK
TST (R1)+ ;POINT TO SPACE FOR EVENT
MOV (SP),(R1)+ ;SAVE EVENT NUMBER IN DEVICE BLOCK
mov #1,oprcnt
MOV #1,GOOPR ;REQUEST THE SERVO PROGRAM TO RUN OPR LVL 6 CODE
1$: EVWAIT (SP) ;WAIT FOR SERVOS TO FINISH
EVKIL ;DELETE EVENT NUMBER
CLR GOOPR ;ZERO ENTRY IN LVL 6 SER. REQ. TABLE
CLR R0 ;NO ERRORS
OPRDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
; OPRLV6 - operates screwdriver or vise
OPRLV6: dec oprcnt
ble 10$
rts pc
10$: MOV OPRDEV,R0 ;POINT TO DEV BLOCK
MOV 4(R0),DAT ;POINT TO SERVO BLOCK
TST @INUSE(DAT) ;CHECK IF SOMEONE ELSE SAYS TO DIE
BMI OP6DNE
TST (DAT) ;DONE YET?
BMI OP6DNE
DEC PTIME(DAT) ;DECREMENT TIME
BLE OP6DNE
1$: BIT #1,PTIME(DAT)
BEQ 4$
JSR PC,READ ;READ VELOCITY
SUB TACHZ0(DAT),R0 ;SUBTRACT TAC ZERO READING
MOV R0,R1 ;SET TO ZERO IF IN NOISE BAND
BGE 7$
NEG R1
7$: BIC #7,R1
BNE 8$
CLR R0
8$: LDCIF R0,AC0
MULF VSCALE(DAT),AC0 ;GET VELOCITY IN DEG/MSEC
STF AC0,THD(DAT) ;AND SAVE
MULF KI(DAT),AC0 ;GET DEGREES TURNED THIS PERIOD DEG/MSEC*MSEC=DEG
ADDF TH(DAT),AC0 ;INTEGRATE TOTAL DEGREES TURNED SINCE START
STF AC0,TH(DAT)
LDF FLEVEL(DAT),AC1 ;ARE WE FORCE SERVOING?
CFCC
BEQ 13$
LDF AC1,AC0
BR 12$
13$: LDF THDP(DAT),AC0
ADDF THD(DAT),AC0 ;COMPUTE ERROR VEL
MULF KV(DAT),AC0 ;*VELOCITY GAIN
12$: CFCC
BLT 5$
ADDF V0(DAT),AC0 ;ADD IN FRICTION TORQUE
BR 6$
5$: SUBF V0(DAT),AC0
6$: MULF MSCALE(DAT),AC0 ;CONVERT TO DAC UNITS
STCFI AC0,R0 ;GET INTEGER DRIVE VALUE
MOV MAXDRV(DAT),R2 ;GET MAX DRIVE IN CASE WE SATURATE
MOV R0,R1 ;CHECK FOR EXCESSIVE TORQUE
BGE 2$
NEG R1 ;GET ABS VALUE OF DRIVE
NEG R2 ;NEG MAX DRIVE
2$: CMP R1,MAXDRV(DAT)
BLE 3$
MOV R2,R0 ;USE MAX DRIVE
DEC COUNT(DAT) ;COUNT SATURATION TIME
BGE 3$
BIS #4000,(DAT) ;SATURATION OCCURED TO LONG
BR OP6DNE ;SO QUIT
3$: BIC #177700,R0 ;REMOVE EXTRA SIGN BITS IF ANY
BIS R0,DACINF(DAT) ;OR IN CHANNEL AND SENSE BIT
BIC #200,DACINF(DAT) ;ENABLE SCR DRIVE
MOV #40,DR11S ;SET MODE
MOV DACINF(DAT),DR11O ;SEND OUTPUT TO INTERFACE
mov #2,oprcnt ;let it run a while
; SLEEP #32. ;LET IT RUN FOR A WHILE
rts pc
; BR OPRLV6
4$: MOV #40,DR11S ;SET MODE
BIS #200,DACINF(DAT) ;DISABLE DRIVE
MOV DACINF(DAT),DR11O ;ZERO DRIVE FOR THIS PASS
mov #1,oprcnt ;let read velocity next time
; SLEEP #8.
rts pc
; BR OPRLV6
OP6DNE: BIC #3,DR11S ;SET MODE AND DISABLE INTERUPT
MOV #160000,DR11O ;SET ZERO DRIVE
MOV OPRDEV,R0
EVSIG 2(R0) ;SIGNAL DONE *K
rts pc
READ: MOV #42,DR11S ;CODE TO READ A/D
MOVB TACCHN(DAT),DR11O
TSTB DR11S ;WAIT TIL DONE
BPL .-4
MOV DR11I,R0
SUB #3777,R0 ;OFFSET SO 0=0
RTS PC
;OPERATE DATA
DATA
oprcnt: .word 0 ;counter for driver servicing
OPRDEV: .WORD 0 ;POINTER TO DEV. BLOCK
OPRBLK: 0 ;POINTER TO COEF. DATA LIST
100. ;ALLOW 4 SECOND OVERLOAD ON MOTOR
0 ;NO START DELAY
0 ;NO POLYNOMIALS
;VARIABLES, CONSTANTS, AND TEMPORARY STORAGE AREA COMMON TO ALL SERVOS
DATA
VELER: .FLT2 0.0 ;VELOCITY ERROR, THETA DOT - THETA DOT PREDICTED
TORQUE: .BLKW 2 ;OUTPUT MOTOR TORQUE
A05TH: .FLT2 0.0 ;5TH ORDER CORRECTION EQUATION
A15TH: .FLT2 0.0
A25TH: .FLT2 0.0
A35TH: .FLT2 10.0
A45TH: .FLT2 -15.0
A55TH: .FLT2 6.0
FP6: .FLT2 6.0
FP10: .FLT2 10.0
FP15: .FLT2 15.0
FP30: .FLT2 30.0
FP60: .FLT2 60.0
FP90: .FLT2 90.0
ONE48T: .FLT2 .020833333
ONE32N: .FLT2 .03125
;SINUSOIDAL TABLE FOR JOINT WOBBLING
WOBTAB: .FLT2 .0000000, .3826834, .7071068, .9238795
.FLT2 1.000000, .9238795, .7071068, .3826834
.FLT2 .0000000,-.3826834,-.7071068,-.9238795
.FLT2 -1.000000,-.9238795,-.7071068,-.3826834
YWOBMG: .FLT2 0.0 ;MAGNITUDE OF YELLOW ARM WOBBLE
BWOBMG: .FLT2 0.0 ; " " BLUE " "
SRV01: ;YELLOW ARM JOINT #1 DATA
.IFNZ NOYELW
NONEX ;JOINT DOESN'T EXIST
.IFF NOYELW
0 ;JOINT EXISTS
0 ;MODBTS
0 ;INUSE
.word 1 ;SRVNUM
.word YELARM ;MECHSM
.WORD 140511,7733 ;TH -Pi
.WORD 140511,7733 ;THP -Pi
.WORD 140511,7733 ;THN -Pi
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 0.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 .20 ;ERRTOL
.FLT2 -.001 ;KV
.FLT2 -50000.0 ;KE
.FLT2 -5.0@9 ;KI
.FLT2 300.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 2.0 ;KKI
.FLT2 -.0005 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -1000.0 ;KKK
.FLT2 1.0 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
0 ;POTCHN see armdoctxt[armted] *k
8. ;TACCHN ditto
DR11O ;DACADD
.BYTE 0 ;DACCHN
.BYTE 0 ;DACVAL DACCHN IN HIGH 3 BITS
104001 ;DACINF 401
.FLT2 .219 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 6 ;DITHER
.FLT2 .07675 ;SCALE
.FLT2 -217.0 ;OFFSET
.FLT2 60.0 ;USTOP
.FLT2 -185.0 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -1.2594@-3 ;VSCALE
3777 ;MAXDRV
5 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
.ENDC
SRV02: ;YELLOW ARM JOINT #2 DATA
.IFNZ NOYELW
NONEX ;JOINT DOESN'T EXIST
.IFF NOYELW
0 ;JOINT EXISTS
0 ;MODBTS
0 ;INUSE
.word 2 ;SRVNUM
.word YELARM ;MECHSM
.WORD 140311,7733 ;TH -Pi/2
.WORD 140311,7733 ;THP -Pi/2
.WORD 140311,7733 ;THN -Pi/2
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 0.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 .10 ;ERRTOL
.FLT2 -.0004 ;KV
.FLT2 -50000.0 ;KE
.FLT2 -1.0@11 ;KI
.FLT2 0.0 ;V0 nonzero value introdueces too much noise
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
1 ;POTCHN
9. ;TACCHN
DR11O ;DACADD
.BYTE 0 ;DACCHN
.BYTE 40 ;DACVAL
104002 ;DACINF 1002
.FLT2 -.175 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 14 ;DITHER
.FLT2 .0531 ;SCALE
.FLT2 -214.4 ;OFFSET
.FLT2 0.0 ;USTOP could be higher
.FLT2 -170.0 ;LSTOP could be lower
.FLT2 10.0 ;STPBND
.FLT2 -.001 ;VSCALE
3777 ;MAXDRV
13 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
.ENDC
SRV03: ;YELLOW ARM JOINT #3 DATA
.IFNZ NOYELW
NONEX ;JOINT DOESN'T EXIST
.IFF NOYELW
0 ;JOINT EXISTS
0 ;MODBTS
0 ;INUSE
.word 3 ;SRVNUM
.word YELARM ;MECHSM
.WORD 41100,0 ;TH 3.0 inches
.WORD 41100,0 ;THP 3.0 inches
.WORD 41100,0 ;THN 3.0 inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 0.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.05 ;ERRTOL
.FLT2 -.002 ;KV
.FLT2 -250.0 ;KE
.FLT2 -1.0E6 ;KI
.FLT2 25.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
2 ;POTCHN
10. ;TACCHN
DR11O ;DACADD
.BYTE 0 ;DACCHN
.BYTE 100 ;DACVAL
104004 ;DACINF 2004
.FLT2 -2.28 ;MSCALE
.WORD 40200, 0 ;TOTQE 1.000000
.WORD 4 ;DITHER
.FLT2 -.00570 ;SCALE
.FLT2 29.4 ;OFFSET
.FLT2 27.5 ;USTOP elect stop
.FLT2 6.50 ;LSTOP
.FLT2 1.0 ;STPBND
.FLT2 .2848@-3 ;VSCALE
3777 ;MAXDRV
13 ;TACHZ0
0 ;WIPERS multiple wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
.ENDC
SRV04: ;YELLOW ARM JOINT #4 DATA
.IFNZ NOYELW
NONEX ;JOINT DOESN'T EXIST
.IFF NOYELW
0 ;JOINT EXISTS
0 ;MODBTS
0 ;INUSE
.word 4 ;SRVNUM
.word YELARM ;MECHSM
.WORD 140311,7733 ;TH -Pi/2
.WORD 140311,7733 ;THP -Pi/2
.WORD 140311,7733 ;THN -Pi/2
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 0.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.15 ;ERRTOL
.FLT2 -.01 ;KV
.FLT2 -4000.0 ;KE
.FLT2 -1.0@9 ;KI
.FLT2 40.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCII
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
3 ;POTCHN
-1. ;TACCHN
DR11O ;DACADD
.BYTE 0 ;DACCHN
.BYTE 140 ;DACVAL CHANNEL 3
104010 ;DACINF 4010
.FLT2 -2.38 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 10 ;DITHER
.FLT2 -.0832 ;SCALE
.FLT2 148.4 ;OFFSET
.FLT2 86.0 ;USTOP mech stop *K
.FLT2 -175.0 ;LSTOP elect stop → need another wiper
.FLT2 10.0 ;STPBND
.FLT2 0.0 ;VSCALE
3777 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
.ENDC
SRV05: ;YELLOW ARM JOINT #5 DATA
.IFNZ NOYELW
NONEX ;JOINT DOESN'T EXIST
.IFF NOYELW
0 ;JOINT EXISTS
0 ;MODBTS
0 ;INUSE
.word 5 ;SRVNUM
.word YELARM ;MECHSM
.WORD 40311,7733 ;TH Pi/2
.WORD 40311,7733 ;THP Pi/2
.WORD 40311,7733 ;THN Pi/2
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 0.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.15 ;ERRTOL
.FLT2 -.01 ;KV
.FLT2 -4000.0 ;KE
.FLT2 -1.0E9 ;KI
.FLT2 50.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
4 ;POTCHN
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 0 ;DACCHN
.BYTE 200 ;DACVAL CHANNEL 4
104020 ;DACINF 10020
.FLT2 -2.87 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 10 ;DITHER
.FLT2 .0700 ;SCALE
.FLT2 -135.79 ;OFFSET
.FLT2 101.0 ;USTOP
.FLT2 -101.0 ;LSTOP
.FLT2 5.0 ;STPBND
.FLT2 0.0 ;VSCALE
3777 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS multiple wipers
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
.ENDC
SRV06: ;YELLOW ARM JOINT #6 DATA
.IFNZ NOYELW
NONEX ;JOINT DOESN'T EXIST
.IFF NOYELW
0 ;JOINT EXISTS
0 ;MODBTS
0 ;INUSE
.word 6 ;SRVNUM
.word YELARM ;MECHSM
.FLT2 0.0 ;TH 0 degrees
.FLT2 0.0 ;THP 0 degrees
.FLT2 0.0 ;THN 0 degrees
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 0.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.7 ;ERRTOL
.FLT2 -.04 ;KV
.FLT2 -400.0 ;KE
.FLT2 -500000.0 ;KI
.FLT2 25.0 ;V0 70.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
5 ;POTCHN 6 FOR WIPER #2
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 0 ;DACCHN
.BYTE 240 ;DACVAL CHANNEL 5
104077 ;DACINF 20040 *D
.FLT2 3.33 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 10 ;DITHER
.FLT2 .080176 ;SCALE .081077 FOR WIPER #2
.FLT2 -141.0 ;OFFSET -326.64 FOR WIPER #2
.FLT2 110.0 ;USTOP
.FLT2 -135.0 ;LSTOP -290.0 WITH 2 wipers working
.FLT2 5.0 ;STPBND
.FLT2 0.0 ;VSCALE
3777 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS MWIP06
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
.WORD 0,0,0,0,0 ;MULT. WIPER DATA
5 ;A/D CHAN WIPER #1
.FLT2 .080176 ;SCALE .081077 FOR WIPER #2
.FLT2 39.0 ;OFFSET -236.64 FOR WIPER #2
6 ;A/D CHAN WIPER #2
.FLT2 -146.64 ;OFFSET
.FLT2 .081077 ;SCALE
0
MWIP06: 5 ;A/D CHAN WIPER #1
.FLT2 .080176 ;SCALE .081077 FOR WIPER #2
.FLT2 39.0 ;OFFSET -236.64 FOR WIPER #2
6 ;A/D CHAN WIPER #2
.FLT2 -146.64 ;OFFSET
.FLT2 .081077 ;SCALE
0
.ENDC
SRV07: ;YELLOW ARM HAND SERVO DATA BLOCK
.IFNZ NOYELW
NONEX ;JOINT DOESN'T EXIST
.IFF NOYELW
0 ;JOINT EXISTS
0 ;MODBTS
0 ;INUSE
.word 7 ;SRVNUM
.word YELHND ;MECHSM
.WORD 40500,0 ;TH 3.0 inches
.WORD 40500,0 ;THP 3.0 inches
.WORD 40500,0 ;THN 3.0 inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 0.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.1 ;ERRTOL
.FLT2 -.001 ;KV
.FLT2 -10000.0 ;KE
.FLT2 -100.0 ;KI
.FLT2 30.0 ;V0
.FLT2 0.0 ;CI
.FLT2 0.0 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.FLT2 0.0 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
7 ;POTCHN
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 0 ;DACCHN
.BYTE 300 ;DACVAL CHANNEL 6
104100 ;DACINF 100 no tach feedback
.FLT2 -.178 ;MSCALE
.WORD 40200, 0 ;TOTQE 1.000000
.WORD 0 ;DITHER
.FLT2 -.001282 ;SCALE
.FLT2 4.35 ;OFFSET
.FLT2 3.8 ;USTOP
.FLT2 -.2 ;LSTOP
.FLT2 .10 ;STPBND
.FLT2 0.0 ;VSCALE
3777 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
.ENDC
SRV08: 0 ;BLUE ARM JOINT #1 DATA
0 ;MODBTS
0 ;INUSE
.word 8. ;SRVNUM
.word BLUARM ;MECHSM
.FLT2 180.0 ;TH
.FLT2 180.0 ;THP
.FLT2 180.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 .10 ;ERRTOL
.FLT2 -.0320 ;KV -.05
.FLT2 -125000.0 ;KE -150000.0
.FLT2 -5.854@11 ;KI -.6@12
.FLT2 222.0 ;V0 600.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.0@6 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 5.0 ;KKI
.FLT2 .03 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -300.0 ;KKK -1000.0
.FLT2 2.0 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
1000. ;POT
0 ;TACH
21 ;POTCHN
20 ;TACCHN
174000 ;DACADD
.BYTE 1 ;DACCHN
.BYTE 0 ;DACVAL
401 ;DACINF
.FLT2 .146092@-1 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE (pi/180)
.WORD 0 ;DITHER
.FLT2 -0.90269997@-1 ;SCALE WAS -.08653846
.FLT2 267.57998 ;OFFSET WAS 259.5289
.FLT2 190.00 ;USTOP
.FLT2 -45.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -1.6570@-4 ;VSCALE
177 ;MAXDRV
10. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV09: 0 ;BLUE ARM JOINT #2 DATA
0 ;MODBTS
0 ;INUSE
.word 9. ;SRVNUM
.word BLUARM ;MECHSM
.FLT2 -90.0 ;TH
.FLT2 -90.0 ;THP
.FLT2 -90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.10 ;ERRTOL
.FLT2 -.0333 ;KV -.06
.FLT2 -1.25@5 ;KE -1.25@5
.FLT2 -9.0@11 ;KI -9.0@11
.FLT2 400.0 ;V0 600.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.44@6 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 10.0 ;KKI
.FLT2 .008 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -100.0 ;KKK -1000.0
.FLT2 2.0 ;KKF was 1.0
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
14 ;POTCHN
10 ;TACCHN
174000 ;DACADD
.BYTE 2 ;DACCHN
.BYTE 0 ;DACVAL
1002 ;DACINF
.FLT2 .5353@-2 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .06686479 ;SCALE
.FLT2 -222.9941 ;OFFSET
.FLT2 -50.00 ;USTOP
.FLT2 -165.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -1.5401@-4 ;VSCALE
177 ;MAXDRV
10. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV10: 0 ;BLUE ARM JOINT #3 DATA
0 ;MODBTS
0 ;INUSE
.word 10. ;SRVNUM
.word BLUARM ;MECHSM
.FLT2 12.0 ;TH inches
.FLT2 12.0 ;THP inches
.FLT2 12.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.02 ;ERRTOL
.FLT2 -0.0537 ;KV -0.03
.FLT2 -500.0 ;KE -200.0
.FLT2 -1.86@7 ;KI -4.0@6
.FLT2 5.0 ;V0 12.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 484940.0 ;CII
.BLKW 2 ;DCII
.FLT2 484940.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .50 ;KKI
.FLT2 .20 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -100.0 ;KKK
.FLT2 1.0 ;KKF 2.0
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
.BLKW 1 ;POT
.BLKW 1 ;TACH
15 ;POTCHN
11 ;TACCHN
174000 ;DACADD
.BYTE 3 ;DACCHN
.BYTE 0 ;DACVAL
2004 ;DACINF
.FLT2 -0.43478 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .9099181@-2 ;SCALE
.FLT2 .8012 ;OFFSET
.FLT2 33.00 ;USTOP
.FLT2 6.75 ;LSTOP
.FLT2 1.0 ;STPBND
.FLT2 3.0457@-5 ;VSCALE
177 ;MAXDRV
1 ;TACHZ0
0 ;WIPERS single
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV11: 0 ;BLUE ARM JOINT #4 DATA
0 ;MODBTS
0 ;INUSE
.word 11. ;SRVNUM
.word BLUARM ;MECHSM
.FLT2 -90.0 ;TH
.FLT2 -90.0 ;THP
.FLT2 -90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.2 ;ERRTOL
.FLT2 -.03 ;KV was -0.0448
.FLT2 -15000.0 ;KE -7000.0
.FLT2 -3.08@9 ;KI -3.0@9
.FLT2 19.0 ;V0 70.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.0@4 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 10.0 ;KKI
.FLT2 .01 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -200.0 ;KKK
.FLT2 .4 ;KKF was .2
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
2 ;POTCHN
6 ;TACCHN
174000 ;DACADD
.BYTE 4 ;DACCHN
.BYTE 0 ;DACVAL
4010 ;DACINF
.FLT2 .9924@-1 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .0925687 ;SCALE
.FLT2 -157.569406 ;OFFSET
.FLT2 205.00 ;USTOP
.FLT2 -295.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -4.0894@-4 ;VSCALE
177 ;MAXDRV
56 ;TACHZ0 was 51.
MWIP11 ;WIPERS multiple wipers
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
;HMMM?
.WORD 0,0,0,0,0 ;MULT. WIPER DATA
2 ;A/D CHAN WIPER #1
.FLT2 -518.07277 ;OFFSET
.FLT2 .0925687 ;SCALE
13 ;A/D CHAN WIPER #2
.FLT2 -322.35 ;OFFSET WAS -327.35677
.FLT2 .0870195 ;SCALE
MWIP11: 2 ;A/D CHAN WIPER #1
.FLT2 -157.569406 ;OFFSET
.FLT2 .0925687 ;SCALE
13 ;A/D CHAN WIPER #2
.FLT2 38.18 ;OFFSET WAS 32.186366
.FLT2 .0870195 ;SCALE
0
SRV12: 0 ;BLUE ARM JOINT #5 DATA
0 ;MODBTS
0 ;INUSE
.word 12. ;SRVNUM
.word BLUARM ;MECHSM
.FLT2 90.0 ;TH
.FLT2 90.0 ;THP
.FLT2 90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.1 ;ERRTOL
.FLT2 -0.0242 ;KV -0.04
.FLT2 -15000.0 ;KE -2000.0
.FLT2 -1.34@9 ;KI -1.0@9
.FLT2 15.0 ;V0 60.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 0.0 ;KKI *K WAS +.1!
.FLT2 .02 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -400.0 ;KKK
.FLT2 .1 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
3 ;POTCHN
7 ;TACCHN
174000 ;DACADD
.BYTE 5 ;DACCHN
.BYTE 0 ;DACVAL
10020 ;DACINF
.FLT2 -.1299540 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .6711409@-1 ;SCALE
.FLT2 -136.9799 ;OFFSET
.FLT2 95.00 ;USTOP
.FLT2 -95.00 ;LSTOP
.FLT2 5.0 ;STPBND
.FLT2 -3.7919@-4 ;VSCALE
177 ;MAXDRV
23. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV13: 0 ;BLUE ARM JOINT #6 DATA
0 ;MODBTS
0 ;INUSE
.word 13. ;SRVNUM
.word BLUARM ;MECHSM
.FLT2 0.0 ;TH
.FLT2 0.0 ;THP
.FLT2 0.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.7 ;ERRTOL
.FLT2 -0.1375 ;KV -0.1
.FLT2 -800.0 ;KE -1000.0
.FLT2 -5.0@7 ;KI -2.18@8
.FLT2 9.0 ;V0 25.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .05 ;KKI
.FLT2 .01 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -10.0 ;KKK
.FLT2 2.0 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
4 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 6 ;DACCHN
.BYTE 0 ;DACVAL
20040 ;DACINF
.FLT2 0.075188 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
5. ;DITHER
.FLT2 -.850260@-1 ;SCALE
.FLT2 223.023 ;OFFSET
.FLT2 200.00 ;USTOP
.FLT2 -110.00 ;LSTOP
.FLT2 5.0 ;STPBND
.BLKW 2 ;VSCALE no tachometer
177 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV14: 0 ;BLUE ARM HAND SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.word 14. ;SRVNUM
.word BLUHND ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.1 ;ERRTOL
.FLT2 -0.06 ;KV
.FLT2 -800.0 ;KE
.FLT2 -0.1@8 ;KI
.FLT2 30.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 -100.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
5 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 7 ;DACCHN
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.FLT2 0.10593 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 -.1358081@-2 ;SCALE
.FLT2 4.344500 ;OFFSET
.FLT2 3.80 ;USTOP
.FLT2 -1.00 ;LSTOP was -.2
.FLT2 0.1 ;STPBND
.BLKW 2 ;VSCALE none
177 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV15: 0 ;VISE SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.word 15. ;SRVNUM
.word VIS ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 40.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.50 ;ERRTOL
.FLT2 -0.06 ;KV
.FLT2 -800.0 ;KE
.FLT2 -0.1@8 ;KI
.FLT2 30.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 1.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
28. ;POTCHN
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 10 ;DACCHN *K
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.FLT2 0.10593 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 1.4432@-3 ;SCALE
.FLT2 -.748 ;OFFSET
.FLT2 4.6875 ;USTOP
.FLT2 -0.10 ;LSTOP
.FLT2 0.1 ;STPBND
.BLKW 2 ;VSCALE none
0 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV16: 0 ;SCREWDRIVER SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.word 16. ;SRVNUM
.word SCRE ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 40.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 1.0@8 ;ERRTOL
.FLT2 .01 ;KV
.FLT2 -800.0 ;KE
.FLT2 40.0 ;KI
.FLT2 4.25 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 0.0 ;VELERR
.FLT2 1.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
-1 ;POTCHN NONE
29. ;TACCHN DRIVER BACK EMF
DR11O ;DACADD YELLOW DAC CONTROL REG
.BYTE 10 ;DACCHN *K
.BYTE 0 ;DACVAL
160000 ;DACINF YELLOW INTERFACE DAC CHAN
.FLT2 1.34 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 -.1358081@-2 ;SCALE
.FLT2 4.344500 ;OFFSET
.FLT2 3.80 ;USTOP
.FLT2 -0.20 ;LSTOP
.FLT2 0.1 ;STPBND
.FLT2 -.0025 ;VSCALE
37 ;MAXDRV
36 ;TACHZ0 was 1
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
SRV17: 0 ;Green PUMA arm data block
0 ;modbts
0 ;inuse
.word 17. ;servo number (this is actually a device)
.word GRNARM ;mechanism
.FLT2 0.0 ;joint angle 1 ** a puma data block differs **
.FLT2 0.0 ;joint angle 2 ** from a regular D.B. in that **
.FLT2 0.0 ;joint angle 3 ** JT1-6 & PPOLY1-6 are defined **
.FLT2 0.0 ;joint angle 4 ** instead of TH - POSERR. **
.FLT2 0.0 ;joint angle 5
.FLT2 0.0 ;joint angle 6
.BLKW 1 ;pointer to polynomial for joint 1
.BLKW 1 ;pointer to polynomial for joint 2
.BLKW 1 ;pointer to polynomial for joint 3
.BLKW 1 ;pointer to polynomial for joint 4
.BLKW 1 ;pointer to polynomial for joint 5
.BLKW 1 ;pointer to polynomial for joint 6
.WORD -1 ;PUMA WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;green PUMA DATPT
.BLKW 1 ;poly (no meaning here: kept for compatibility)
.BLKW 1 ;fci (no meaning here: kept for compatibility)
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
;Here's where the unique PUMA data starts...
0 ;BADPJT
.WORD 763760 ;DRADDR - Address of DR11C for this arm.
.WORD GTH ;ANGADR - Address of associated joint angles.
.ASCIZ /GREEN/ ;PNAME - Name of the arm (3 words)
.BLKW 6 ;PWORK6 - For 6 encoder counts, etc.
.WORD GW1,GW2,GW3,GW4,GW5,GW6 ;JANGLE - WORK ANGLES OR ANYTHING ELSE
.WORD GL1,GL2,GL3,GL4,GL5,GL6 ;LASTTH - LAST ANGLES PUMA MOVED TO.
.WORD 0 ;ARMBIT - What bits are on in this arm's interface.
.WORD 0 ;CALFLG - 0=arm is not calibrated yet.
.WORD 0 ;CTIME - where we are in calibration stage.
.WORD 0 ;SRVERR - which joint is in error.
.FLT2 4.6400 ;EFIRST: Degrees at first zero index.
.FLT2 -87.5249
.FLT2 97.3401
.FLT2 5.2
.FLT2 7.2325
.FLT2 9.6055
.FLT2 -2.1468E-2 ;ADCSCL: Degrees per ADC count, over 128.
.FLT2 1.2333E-2
.FLT2 -2.4632E-2
.FLT2 1.8000E-2
.FLT2 1.9051E-2
.FLT2 1.7895E-2
.FLT2 347.95 ;ADCOFS - Degrees when ADC is (or would be) zero
.FLT2 -290.42
.FLT2 492.92
.FLT2 -295.00
.FLT2 -306.24
.FLT2 -234.78
;End of data block.
GW1: .BLKW 2 ;WORK LOCATION FOR A FLOATING-PT ANGLE.
GW2: .BLKW 2
GW3: .BLKW 2
GW4: .BLKW 2
GW5: .BLKW 2
GW6: .BLKW 2
GL1: .BLKW 2
GL2: .BLKW 2
GL3: .BLKW 2
GL4: .BLKW 2
GL5: .BLKW 2
GL6: .BLKW 2
SRV18: ;green hand servo data block
.IFNZ NOPHND ;If no puma hands
NONEX ;JOINT DOESN'T EXIST
.IFF NOPHND
0
0 ;MODBTS
0 ;INUSE
.word 18. ;SRVNUM
.word GRNHND ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.ENDC
SRV19: 0 ;Red PUMA arm data block
0 ;modbts
0 ;inuse
.word 19. ;servo number (this is actually a device)
.word REDARM ;mechanism
.FLT2 0.0 ;joint angle 1 ** a puma data block differs **
.FLT2 0.0 ;joint angle 2 ** from a regular D.B. in that **
.FLT2 0.0 ;joint angle 3 ** JT1-6 & PPOLY1-6 are defined **
.FLT2 0.0 ;joint angle 4 ** instead of TH - POSERR. **
.FLT2 0.0 ;joint angle 5
.FLT2 0.0 ;joint angle 6
.BLKW 1 ;pointer to polynomial for joint 1
.BLKW 1 ;pointer to polynomial for joint 2
.BLKW 1 ;pointer to polynomial for joint 3
.BLKW 1 ;pointer to polynomial for joint 4
.BLKW 1 ;pointer to polynomial for joint 5
.BLKW 1 ;pointer to polynomial for joint 6
.WORD -1 ;PUMA WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;red PUMA DATPT
.BLKW 1 ;poly (no meaning here: kept for compatibility)
.BLKW 1 ;fci (no meaning here: kept for compatibility)
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
;Here's where the unique PUMA data starts...
0 ;BADPJT
.WORD 763770 ;DRADDR - Address of DR11C for this arm.
.WORD RTH ;ANGADR - Address of associated joint angles.
.ASCIZ /RED/ ;PNAME - Name of the arm (3 words)
.BYTE 0,0 ;THE 3RD WORD OF PNAME
.BLKW 6 ;PWORK6 - For 6 encoder counts, etc.
.WORD RW1,RW2,RW3,RW4,RW5,RW6 ;JANGLE - WORK ANGLES OR ANYTHING ELSE
.WORD RL1,RL2,RL3,RL4,RL5,RL6 ;LASTTH - LAST ANGLES PUMA MOVED TO.
.WORD 0 ;ARMBIT - What bits are on in this arm's interface.
.WORD 0 ;CALFLG - 0=arm is not calibrated yet.
.WORD 0 ;CTIME - where we are in calibration stage.
.WORD 0 ;SRVERR - which joint is in error.
.FLT2 4.3295 ;EFIRST: Degrees at first zero index.
.FLT2 -85.2961
.FLT2 99.1969
.FLT2 5.2222
.FLT2 8.9043
.FLT2 7.0791
.FLT2 -2.2027E-2 ;ADCSCL: Degrees per ADC count, over 128.
.FLT2 1.2631E-2
.FLT2 -2.5527E-2
.FLT2 1.8029E-2
.FLT2 1.9104E-2
.FLT2 1.8086E-2
.FLT2 362.87 ;ADCOFS: Degrees when ADC is (or would be) zero
.FLT2 -295.72
.FLT2 509.31
.FLT2 -293.29
.FLT2 -313.32
.FLT2 -213.74
;End of data block.
RW1: .BLKW 2 ;WORK LOCATION FOR A FLOATING-PT ANGLE.
RW2: .BLKW 2
RW3: .BLKW 2
RW4: .BLKW 2
RW5: .BLKW 2
RW6: .BLKW 2
RL1: .BLKW 2
RL2: .BLKW 2
RL3: .BLKW 2
RL4: .BLKW 2
RL5: .BLKW 2
RL6: .BLKW 2
SRV20: ;Red hand servo data block
.IFNZ NOPHND ;If no puma hands
NONEX ;JOINT DOESN'T EXIST
.IFF NOPHND
0
0 ;MODBTS
0 ;INUSE
.word 20. ;SRVNUM
.word REDHND ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.ENDC
;PUMA servo constants for microprocessor-based joint servos:
ENBMDE: .WORD 700 ;Turn on with integration enabled.
.WORD 700,700,700,700,720
ENBNIT: .WORD 600 ;Turn on without integration.
.WORD 600,600,600,600,620
CALMDE: .WORD 701 ;Calibration mode
.WORD 701,701,701,701,721
STOPJT: .WORD 0,0,0,0,0,0 ;Doesn't really use this data for a STOP command
CALMVE: .FLT2 0.05 ;Increment angle to move each joint during calib
.FLT2 0.03, 0.06, 0.05, 0.05, 0.08
ZICNT: .WORD 4750 ;Ditto: Addr 1001=11, data 11101000=350
.WORD 4440 ; Addr 1001=11, data 00100000=040
.WORD 4750 ; Addr 11, data 350
.WORD 4750, 4750, 4750
VELGAN: .WORD 56001 ;Velocity gains: Addr 1011100=134, data 001
.WORD 56001 ;data 001
.WORD 56002 ;data 002
.WORD 56003 ;data 003
.WORD 56003 ;data 003
.WORD 56003 ;data 003
DIVMDE: .WORD 400 ;Set divide by two bit for encoders.
.WORD 400 ;Addr = 1, data = 0
.WORD 400,400,400,420
INTRGE: .WORD 50. ;Hardware integration region
.WORD 50., 50. ,50., 50., 50.
TOLRGE: .WORD 40. ;Narrow in range tolerance limits.
.WORD 40., 40., 40., 40., 40.
WIDTOL: .WORD 200. ;Wide tolerance range
.WORD 200., 200., 200., 200., 200.
; Arm Constants & Conversion Factors -- Same for both PUMA arms.
ESCALE: .FLT2 -173.92 ;How many encoder counts per degree
.FLT2 239.59
.FLT2 -149.18
.FLT2 211.211
.FLT2 199.79
.FLT2 106.51
EOFFST: .FLT2 0.0 ;Angles when encoder is zero (or 100000)
.FLT2 -90.0
.FLT2 90.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
EDELTA: .FLT2 -5.7498 ;Degrees between each zero index.
.FLT2 3.3390
.FLT2 -6.7033
.FLT2 4.7346
.FLT2 -5.0053
.FLT2 9.3888
CFACTR: .FLT2 -0.180556 ;Coupling factor between joints 4 & 5
MAXDIF: .FLT2 1.0 ;Maximum angle error allowed in positioning
.FLT2 1.0
.FLT2 1.0
.FLT2 1.0
.FLT2 1.0
.FLT2 1.0
SPDTHR: .FLT2 0.05 ;Speed threshold for collision detection
.FLT2 0.05, 0.05, 0.05, 0.05, 0.05
;DIAGNOSTIC DATA BUFFER: ORGANIZATION AND LOCAL STORAGE
;THE DIAGNOSTIC DATA ARRAY IS OF THE FOLLOWING FORM
;
; DBUF: LAST WORD
; +2 : PTIME ;TIME SINCE THE START OF CURRENT SEGMENT(MSEC)
; +4 : TH|POSERR ;JOINT ANGLE FROM A/D READING(F.P.)
; +10 : THD|VELERR ;JOINT VELOCITY FROM A/D (F.P.)
; +14 : DACOUT ;OUTPUT MOTOR DAC
;
;THE SEQUENCE REPEATS STARTING WITH SRVNUM AGAIN. THE POINTER "DBUF" IS
;ALWAYS LEFT POINTING AT THE FIRST UNUSED WORD IN THE DIAGNOSTIC BUFFER.
DBUF:
.IFNZ DIAGY
.+2 ;STORAGE AREA FOR DIAGNOSTIC JOINT DATA
.BLKW DBUFL
ACTUAL: 1 ;SET 0 = SEND ERROR TERMS, 1 = ACTUAL ADC READINGS
.ENDC
.IFNZ TACCAL
BELL: .BYTE 7,0
.ENDC
.IFNZ TIMER
TIMES: .BLKW 1000. ;BUFFER AREA TO SAVE SERVO EXECUTION TIME
.ENDC
GIAREA: .BLKW 4. ;INTEGER STORAGE FOR GATHER
GAREA: 0 ; changed to 0 by MSM
READW: 0 ; CHANGED TO 0 BY MSM
ARMEND: